From 1e778746113e410b3ce33d925ddcbfbc81031bfd Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 1 Mar 2023 12:30:05 -0800 Subject: [PATCH 001/153] first draft of collision_shape_automation.py --- examples/viewer.py | 116 +++++++- tools/collision_shape_automation.py | 406 ++++++++++++++++++++++++++++ 2 files changed, 520 insertions(+), 2 deletions(-) create mode 100644 tools/collision_shape_automation.py diff --git a/examples/viewer.py b/examples/viewer.py index 78df8231ba..a277571e6f 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -25,6 +25,17 @@ from habitat_sim.utils.common import quat_from_angle_axis from habitat_sim.utils.settings import default_sim_settings, make_cfg +# add tools directory so I can import things to try them in the viewer +sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../tools")) +print(sys.path) +import collision_shape_automation as csa + +gt_raycast_results = None +pr_raycast_results = None +obj_temp_handle = None +test_points = None +normalized_error = 0 + class HabitatSimInteractiveViewer(Application): # the maximum number of chars displayable in the app window @@ -166,7 +177,10 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.previous_mouse_point = None # toggle physics simulation on/off - self.simulating = True + self.simulating = False + self.sample_seed = 0 + self.collision_proxy_obj = None + self.mouse_cast_results = None # toggle a single simulation step at the next opportunity if not # simulating continuously. @@ -192,6 +206,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: LoggingContext.reinitialize_from_env() logger.setLevel("INFO") self.print_help_text() + print(f"Normalized Error = {normalized_error}") def draw_contact_debug(self, debug_line_render: Any): """ @@ -262,6 +277,51 @@ def debug_draw(self): ) self.draw_region_debug(debug_line_render) + if gt_raycast_results is not None: + scene_bb = self.sim.get_active_scene_graph().get_root_node().cumulative_bb + inflated_scene_bb = scene_bb.scaled(mn.Vector3(1.25)) + inflated_scene_bb = mn.Range3D.from_center( + scene_bb.center(), inflated_scene_bb.size() / 2.0 + ) + white = mn.Color4(mn.Vector3(1.0), 1.0) + self.sim.get_debug_line_render().draw_box( + inflated_scene_bb.min, inflated_scene_bb.max, white + ) + if self.sim.get_rigid_object_manager().get_num_objects() == 0: + self.collision_proxy_obj = ( + self.sim.get_rigid_object_manager().add_object_by_template_handle( + obj_temp_handle + ) + ) + + csa.debug_draw_raycast_results( + self.sim, gt_raycast_results, pr_raycast_results, seed=self.sample_seed + ) + + # draw test points + for side in test_points: + for p in side: + self.sim.get_debug_line_render().draw_circle( + translation=p, + radius=0.005, + color=mn.Color4.yellow(), + ) + + if ( + self.mouse_cast_results is not None + and self.mouse_cast_results.has_hits() + ): + m_ray = self.mouse_cast_results.ray + self.sim.get_debug_line_render().draw_circle( + translation=m_ray.origin + + m_ray.direction + * self.mouse_cast_results.hits[0].ray_distance + * m_ray.direction.length(), + radius=0.005, + color=white, + normal=self.mouse_cast_results.hits[0].normal, + ) + def draw_event( self, simulation_call: Optional[Callable] = None, @@ -383,6 +443,28 @@ def reconfigure_sim(self) -> None: self.cfg.sim_cfg.override_scene_light_defaults = True self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY + # create custom stage from object + self.cfg.metadata_mediator = habitat_sim.metadata.MetadataMediator() + self.cfg.metadata_mediator.active_dataset = self.sim_settings[ + "scene_dataset_config_file" + ] + otm = self.cfg.metadata_mediator.object_template_manager + obj_template = otm.get_template_by_handle(obj_temp_handle) + obj_template.compute_COM_from_shape = False + obj_template.com = mn.Vector3(0) + otm.register_template(obj_template) + stm = self.cfg.metadata_mediator.stage_template_manager + stage_template_name = "obj_as_stage_template" + new_stage_template = stm.create_new_template(handle=stage_template_name) + new_stage_template.render_asset_handle = obj_template.render_asset_handle + stm.register_template( + template=new_stage_template, specified_handle=stage_template_name + ) + self.cfg.sim_cfg.scene_id = stage_template_name + # visualize the object as its collision shape + obj_template.render_asset_handle = obj_template.collision_asset_handle + otm.register_template(obj_template) + if self.sim is None: self.tiled_sims = [] for _i in range(self.num_env): @@ -572,7 +654,22 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.contact_debug_draw = True # TODO: add a nice log message with concise contact pair naming. + elif key == pressed.O: + # move the object in/out of the frame + if self.collision_proxy_obj is not None: + if self.collision_proxy_obj.translation == mn.Vector3(0): + self.collision_proxy_obj.translation = mn.Vector3(100) + else: + self.collision_proxy_obj.translation = mn.Vector3(0) + elif key == pressed.T: + if shift_pressed: + self.sample_seed -= 1 + else: + self.sample_seed += 1 + + event.accepted = True + return # load URDF fixed_base = alt_pressed urdf_file_path = "" @@ -680,6 +777,11 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: mouse button to steer the agent's facing direction. When in GRAB mode, continues to update the grabber's object position with our agents position. """ + + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(event.position)) + self.mouse_cast_results = self.sim.cast_ray(ray=ray) + button = Application.MouseMoveEvent.Buttons # if interactive mode -> LOOK MODE if event.buttons == button.LEFT and self.mouse_interaction == MouseMode.LOOK: @@ -1208,7 +1310,8 @@ def next_frame() -> None: # Setting up sim_settings sim_settings: Dict[str, Any] = default_sim_settings - sim_settings["scene"] = args.scene + # sim_settings["scene"] = args.scene + sim_settings["scene"] = "NONE" sim_settings["scene_dataset_config_file"] = args.dataset sim_settings["enable_physics"] = not args.disable_physics sim_settings["use_default_lighting"] = args.use_default_lighting @@ -1220,5 +1323,14 @@ def next_frame() -> None: sim_settings["default_agent_navmesh"] = False sim_settings["enable_hbao"] = args.hbao + obj_name = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" + ( + gt_raycast_results, + pr_raycast_results, + obj_temp_handle, + test_points, + normalized_error, + ) = csa.evaluate_collision_shape(obj_name, sim_settings) + # start the application HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py new file mode 100644 index 0000000000..de4d6db18b --- /dev/null +++ b/tools/collision_shape_automation.py @@ -0,0 +1,406 @@ +# 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 argparse +import random +from typing import Any, Dict, List, Tuple + +import magnum as mn +import numpy as np + +import habitat_sim +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +# object samples: +# chair - good approximation: 0a5e809804911e71de6a4ef89f2c8fef5b9291b3.glb +# shelves - bad approximation: d1d1e0cdaba797ee70882e63f66055675c3f1e7f.glb + + +def sample_points_from_range3d( + range3d: mn.Range3D, num_points: int = 100 +) -> List[List[mn.Vector3]]: + """ + Sample 'num_points' from the surface of a box defeined by 'range3d'. + """ + + # ----------------------------------------- + # area weighted face sampling + face_areas = [ + range3d.size_x() * range3d.size_y(), # front/back + range3d.size_x() * range3d.size_z(), # top/bottom + range3d.size_y() * range3d.size_z(), # sides + ] + area_accumulator = [] + for ix in range(6): + area_ix = ix % 3 + if ix == 0: + area_accumulator.append(face_areas[area_ix]) + else: + area_accumulator.append(face_areas[area_ix] + area_accumulator[-1]) + + normalized_area_accumulator = [x / area_accumulator[-1] for x in area_accumulator] + print(normalized_area_accumulator) + + def sample_face() -> int: + """ + Weighted sampling of a face from the area accumulator. + """ + rand = random.random() + for ix in range(6): + if normalized_area_accumulator[ix] > rand: + return ix + raise (AssertionError, "Should not reach here.") + + # ----------------------------------------- + + # For each face a starting point and two edge vectors (un-normalized) + face_info: List[Tuple[mn.Vector3, mn.Vector3, mn.Vector3]] = [ + ( + range3d.front_bottom_left, + mn.Vector3.x_axis(range3d.size_x()), + mn.Vector3.y_axis(range3d.size_y()), + ), # front + ( + range3d.back_top_left, + mn.Vector3.x_axis(range3d.size_x()), + mn.Vector3.z_axis(range3d.size_z()), + ), # top + ( + range3d.back_bottom_left, + mn.Vector3.y_axis(range3d.size_y()), + mn.Vector3.z_axis(range3d.size_z()), + ), # left + ( + range3d.back_bottom_left, + mn.Vector3.x_axis(range3d.size_x()), + mn.Vector3.y_axis(range3d.size_y()), + ), # back + ( + range3d.back_bottom_left, + mn.Vector3.x_axis(range3d.size_x()), + mn.Vector3.z_axis(range3d.size_z()), + ), # bottom + ( + range3d.back_bottom_right, + mn.Vector3.y_axis(range3d.size_y()), + mn.Vector3.z_axis(range3d.size_z()), + ), # right + ] + + # one internal list of each face of the box: + samples = [] + for _ in range(6): + samples.append([]) + + # sample points for random faces + for _ in range(num_points): + face_ix = sample_face() + f = face_info[face_ix] + point = f[0] + random.random() * f[1] + random.random() * f[2] + samples[face_ix].append(point) + + return samples + + +def sample_points_from_sphere( + center: mn.Vector3, radius: float, num_points: int = 100 +) -> List[List[mn.Vector3]]: + """ + Sample num_points from a sphere defined by center and radius. + Return all points in two identical lists to indicate pairwise raycasting. + """ + samples = [] + + # sample points + while len(samples) < num_points: + # rejection sample unit sphere from volume + rand_point = np.random.random(3) * 2.0 - np.ones(1) + vec_len = np.linalg.norm(rand_point) + if vec_len < 1.0: + # inside the sphere, so project to the surface + samples.append(mn.Vector3(rand_point / vec_len)) + # else outside the sphere, so rejected + + # move from unit sphere to input sphere + samples = [x * radius + center for x in samples] + + # collect into pairwise datastructure + samples = [samples, samples] + + return samples + + +def run_pairwise_raycasts( + points: List[List[mn.Vector3]], sim: habitat_sim.Simulator, min_dist: float = 0.1 +) -> List[habitat_sim.physics.RaycastResults]: + """ + Raycast between each pair of points from different surfaces. + :param min_dist: The minimum ray distance to allow. Cull all candidate pairs closer than this distance. + """ + all_raycast_results: List[habitat_sim.physics.RaycastResults] = [] + for fix0 in range(len(points)): + for fix1 in range(len(points)): + if fix0 != fix1: # no pairs on the same face + for p0 in points[fix0]: + for p1 in points[fix1]: + if (p0 - p1).length() > min_dist: + # this is a valid pair of points + ray = habitat_sim.geo.Ray(p0, p1 - p0) # origin, direction + # raycast + all_raycast_results.append(sim.cast_ray(ray=ray)) + # reverse direction as separate entry (because exiting a convex does not generate a hit record) + ray2 = habitat_sim.geo.Ray(p1, p0 - p1) # origin, direction + # raycast + all_raycast_results.append(sim.cast_ray(ray=ray2)) + + return all_raycast_results + + +def debug_draw_raycast_results( + sim, ground_truth_results, proxy_results, subsample_number: int = 100, seed=0 +) -> None: + """ + Render debug lines for a subset of raycast results, randomly subsampled for efficiency. + """ + random.seed(seed) + red = mn.Color4.red() + yellow = mn.Color4.yellow() + blue = mn.Color4.blue() + grey = mn.Color4(mn.Vector3(0.6), 1.0) + for _ in range(subsample_number): + result_ix = random.randint(0, len(ground_truth_results) - 1) + ray = ground_truth_results[result_ix].ray + gt_results = ground_truth_results[result_ix] + pr_results = proxy_results[result_ix] + + if gt_results.has_hits() or pr_results.has_hits(): + # some logic for line colors + first_hit_dist = 0 + # pairs of distances for overshooting the ground truth and undershooting the ground truth + overshoot_dists = [] + undershoot_dists = [] + + if gt_results.has_hits(): + # draw first and last hits for gt and proxy + sim.get_debug_line_render().draw_circle( + translation=ray.origin + + ray.direction * gt_results.hits[0].ray_distance, + radius=0.005, + color=blue, + normal=gt_results.hits[0].normal, + ) + if pr_results.has_hits(): + sim.get_debug_line_render().draw_circle( + translation=ray.origin + + ray.direction * pr_results.hits[0].ray_distance, + radius=0.005, + color=yellow, + normal=pr_results.hits[0].normal, + ) + + if not gt_results.has_hits(): + first_hit_dist = pr_results.hits[0].ray_distance + overshoot_dists.append((first_hit_dist, 1.0)) + elif not pr_results.has_hits(): + first_hit_dist = gt_results.hits[0].ray_distance + undershoot_dists.append((first_hit_dist, 1.0)) + else: + # both have hits + first_hit_dist = min( + gt_results.hits[0].ray_distance, pr_results.hits[0].ray_distance + ) + + # compute overshoots and undershoots for first hit: + if gt_results.hits[0].ray_distance < pr_results.hits[0].ray_distance: + # undershoot + undershoot_dists.append( + ( + gt_results.hits[0].ray_distance, + pr_results.hits[0].ray_distance, + ) + ) + else: + # overshoot + overshoot_dists.append( + ( + gt_results.hits[0].ray_distance, + pr_results.hits[0].ray_distance, + ) + ) + + # draw blue lines for overlapping distances + sim.get_debug_line_render().draw_transformed_line( + ray.origin, ray.origin + ray.direction * first_hit_dist, blue + ) + + # draw red lines for overshoots (proxy is outside the ground truth) + for d0, d1 in overshoot_dists: + sim.get_debug_line_render().draw_transformed_line( + ray.origin + ray.direction * d0, + ray.origin + ray.direction * d1, + red, + ) + + # draw yellow lines for undershoots (proxy is inside the ground truth) + for d0, d1 in undershoot_dists: + sim.get_debug_line_render().draw_transformed_line( + ray.origin + ray.direction * d0, + ray.origin + ray.direction * d1, + yellow, + ) + + else: + # no hits, grey line + sim.get_debug_line_render().draw_transformed_line( + ray.origin, ray.origin + ray.direction, grey + ) + + +def get_raycast_results_cumulative_error_metric( + ground_truth_results, proxy_results +) -> float: + """ + Generates a scalar error metric from raycast results normalized to [0,1]. + + absolute_error = sum(abs(gt_1st_hit_dist-pr_1st_hit_dist)) + + To normalize error: + 0 corresponds to gt distances (absolute_error == 0) + 1 corresponds to max error. For each ray, max error is max(gt_1st_hit_dist, ray_length-gt_1st_hit_dist). + max_error = sum(max(gt_1st_hit_dist, ray_length-gt_1st_hit_dist)) + normalized_error = error/max_error + """ + assert len(ground_truth_results) == len( + proxy_results + ), "raycast results must be equivalent." + + max_error = 0 + absolute_error = 0 + for r_ix in range(len(ground_truth_results)): + ray = ground_truth_results[r_ix].ray + ray_len = ray.direction.length() + local_max_error = ray_len + gt_dist = ray_len + if ground_truth_results[r_ix].has_hits(): + gt_dist = ground_truth_results[r_ix].hits[0].ray_distance + local_max_error = max(gt_dist, ray_len - gt_dist) + max_error += local_max_error + local_absolute_error = ray_len + if proxy_results[r_ix].has_hits(): + local_absolute_error = abs( + proxy_results[r_ix].hits[0].ray_distance - gt_dist + ) + absolute_error += local_absolute_error + + normalized_error = absolute_error / max_error + return normalized_error + + +def evaluate_collision_shape( + object_handle: str, sim_settings: Dict[str, Any], sample_shape="sphere" +) -> None: + """ + Runs in-engine evaluation of collision shape accuracy for a single object. + Uses a raycast from a bounding shape to approximate surface error between a proxy shape and ground truth (the render shape). + Returns: + ground_truth and proxy raw raycast results, + the object's template handle, + all test points used for the raycast, + scalar error metrics + 1. initializes a simulator with the object render shape as a stage collision mesh. + 2. uses the scene bouding box to sample points on a configured bounding shape (e.g. inflated AABB or sphere). + 3. raycasts between sampled point pairs on both ground truth and collision proxy shapes to heuristicall measure error. + 4. computes scalar error metrics + + :param object_handle: The object to evaluate. + :param sim_settings: Any simulator settings for initialization (should be physics enabled and point to correct dataset). + :param sample_shape: The desired bounding shape for raycast: "sphere" or "aabb". + """ + cfg = make_cfg(sim_settings) + with habitat_sim.Simulator(cfg) as sim: + # evaluate the collision shape defined in an object's template + # 1. get the template from MM + matching_templates = sim.get_object_template_manager().get_template_handles( + object_handle + ) + assert ( + len(matching_templates) == 1 + ), f"Multiple or no template matches for handle '{object_handle}': ({matching_templates})" + obj_template_handle = matching_templates[0] + obj_template = sim.get_object_template_manager().get_template_by_handle( + obj_template_handle + ) + obj_template.compute_COM_from_shape = False + obj_template.com = mn.Vector3(0) + obj_template.bounding_box_collisions = True + obj_template.is_collidable = False + sim.get_object_template_manager().register_template(obj_template) + # 2. Setup a stage from the object's render mesh + stm = sim.get_stage_template_manager() + stage_template_name = "obj_as_stage_template" + new_stage_template = stm.create_new_template(handle=stage_template_name) + new_stage_template.render_asset_handle = obj_template.render_asset_handle + stm.register_template( + template=new_stage_template, specified_handle=stage_template_name + ) + # 3. Initialize the simulator for the stage + new_settings = sim_settings.copy() + new_settings["scene"] = stage_template_name + new_config = make_cfg(new_settings) + sim.reconfigure(new_config) + # 4. compute initial metric baselines + scene_bb = sim.get_active_scene_graph().get_root_node().cumulative_bb + inflated_scene_bb = scene_bb.scaled(mn.Vector3(1.25)) + inflated_scene_bb = mn.Range3D.from_center( + scene_bb.center(), inflated_scene_bb.size() / 2.0 + ) + # bounding box sample + # test_points = sample_points_from_range3d(range3d=inflated_scene_bb) + # bounding sphere sample + half_diagonal = (scene_bb.max - scene_bb.min).length() / 2.0 + test_points = sample_points_from_sphere( + center=inflated_scene_bb.center(), radius=half_diagonal + ) + gt_raycast_results = run_pairwise_raycasts(test_points, sim) + # 5. load the object with proxy (in NONE stage) + new_settings = sim_settings.copy() + new_config = make_cfg(new_settings) + sim.reconfigure(new_config) + obj = sim.get_rigid_object_manager().add_object_by_template_handle( + obj_template_handle + ) + obj.translation = obj.com + # 6. compute the metric for proxy object + pr_raycast_results = run_pairwise_raycasts(test_points, sim) + # 7. compare metrics + normalized_error = get_raycast_results_cumulative_error_metric( + gt_raycast_results, pr_raycast_results + ) + return ( + gt_raycast_results, + pr_raycast_results, + obj_template_handle, + test_points, + normalized_error, + ) + + +def main(): + parser = argparse.ArgumentParser( + description="Automate collision shape creation and validation." + ) + parser.add_argument("--dataset", type=str, help="path to SceneDataset.") + parser.add_argument( + "--object-handle", type=str, help="handle identifying the object to evaluate." + ) + args = parser.parse_args() + + sim_settings = default_sim_settings.copy() + sim_settings["scene_dataset_config_file"] = args.dataset + + evaluate_collision_shape(args.object_handle, sim_settings) + + +if __name__ == "__main__": + main() From 32fc61958064b5558c5e242259376ad55f716753 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 6 Mar 2023 15:00:24 -0800 Subject: [PATCH 002/153] profiling, refactors, efficiency improvements, stats --- examples/viewer.py | 51 ++++++++++++--- tools/collision_shape_automation.py | 96 ++++++++++++++++++++++------- 2 files changed, 118 insertions(+), 29 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index a277571e6f..fcd98d287b 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1324,13 +1324,50 @@ def next_frame() -> None: sim_settings["enable_hbao"] = args.hbao obj_name = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" - ( - gt_raycast_results, - pr_raycast_results, - obj_temp_handle, - test_points, - normalized_error, - ) = csa.evaluate_collision_shape(obj_name, sim_settings) + + # load JSON once instead of repeating + mm = habitat_sim.metadata.MetadataMediator() + mm.active_dataset = sim_settings["scene_dataset_config_file"] + + num_point_samples = 1000 + sample_metrics = [] + avg_metric = 0 + avg_profile_metrics = {} + for _sample in range(5): + ( + gt_raycast_results, + pr_raycast_results, + obj_temp_handle, + test_points, + normalized_error, + profile_metrics, + ) = csa.evaluate_collision_shape( + obj_name, + sim_settings, + sample_shape="aabb", + mm=mm, + num_point_samples=num_point_samples, + ) + for key in list(profile_metrics.keys()): + if key not in avg_profile_metrics: + avg_profile_metrics[key] = profile_metrics[key] + else: + avg_profile_metrics[key] += profile_metrics[key] + + sample_metrics.append(normalized_error) + avg_metric += normalized_error + for key in list(avg_profile_metrics.keys()): + avg_profile_metrics[key] = avg_profile_metrics[key] / len(sample_metrics) + avg_metric /= len(sample_metrics) + print(f"sample_metrics ({len(sample_metrics)} samples) = {sample_metrics}") + print(f"avg_metric = {avg_metric}") + variance = 0 + for metric in sample_metrics: + variance += (metric - avg_metric) ** 2 + variance /= len(sample_metrics) + print(f"variance = {variance}") + print(f"avg_profile_metrics = {avg_profile_metrics}") + exit() # start the application HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index de4d6db18b..52f4de6537 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -4,6 +4,7 @@ import argparse import random +import time from typing import Any, Dict, List, Tuple import magnum as mn @@ -17,15 +18,11 @@ # shelves - bad approximation: d1d1e0cdaba797ee70882e63f66055675c3f1e7f.glb -def sample_points_from_range3d( - range3d: mn.Range3D, num_points: int = 100 -) -> List[List[mn.Vector3]]: +def compute_area_weights_for_range3d_faces(range3d: mn.Range3D): """ - Sample 'num_points' from the surface of a box defeined by 'range3d'. + Compute a set of area weights from a Range3D. """ - # ----------------------------------------- - # area weighted face sampling face_areas = [ range3d.size_x() * range3d.size_y(), # front/back range3d.size_x() * range3d.size_z(), # top/bottom @@ -40,7 +37,20 @@ def sample_points_from_range3d( area_accumulator.append(face_areas[area_ix] + area_accumulator[-1]) normalized_area_accumulator = [x / area_accumulator[-1] for x in area_accumulator] - print(normalized_area_accumulator) + + return normalized_area_accumulator + + +def sample_points_from_range3d( + range3d: mn.Range3D, num_points: int = 100 +) -> List[List[mn.Vector3]]: + """ + Sample 'num_points' from the surface of a box defeined by 'range3d'. + """ + + # ----------------------------------------- + # area weighted face sampling + normalized_area_accumulator = compute_area_weights_for_range3d_faces(range3d) def sample_face() -> int: """ @@ -283,14 +293,13 @@ def get_raycast_results_cumulative_error_metric( local_max_error = ray_len gt_dist = ray_len if ground_truth_results[r_ix].has_hits(): - gt_dist = ground_truth_results[r_ix].hits[0].ray_distance + gt_dist = ground_truth_results[r_ix].hits[0].ray_distance * ray_len local_max_error = max(gt_dist, ray_len - gt_dist) max_error += local_max_error - local_absolute_error = ray_len + local_proxy_dist = ray_len if proxy_results[r_ix].has_hits(): - local_absolute_error = abs( - proxy_results[r_ix].hits[0].ray_distance - gt_dist - ) + local_proxy_dist = proxy_results[r_ix].hits[0].ray_distance * ray_len + local_absolute_error = abs(local_proxy_dist - gt_dist) absolute_error += local_absolute_error normalized_error = absolute_error / max_error @@ -298,7 +307,11 @@ def get_raycast_results_cumulative_error_metric( def evaluate_collision_shape( - object_handle: str, sim_settings: Dict[str, Any], sample_shape="sphere" + object_handle: str, + sim_settings: Dict[str, Any], + sample_shape="sphere", + mm=None, + num_point_samples=100, ) -> None: """ Runs in-engine evaluation of collision shape accuracy for a single object. @@ -316,9 +329,17 @@ def evaluate_collision_shape( :param object_handle: The object to evaluate. :param sim_settings: Any simulator settings for initialization (should be physics enabled and point to correct dataset). :param sample_shape: The desired bounding shape for raycast: "sphere" or "aabb". + :param mm: A pre-configured MetadataMediator may be provided to reduce initialization time. Should have the correct dataset already configured. """ + profile_metrics = {} + start_time = time.time() + check_time = time.time() cfg = make_cfg(sim_settings) + if mm is not None: + cfg.metadata_mediator = mm with habitat_sim.Simulator(cfg) as sim: + profile_metrics["init0"] = time.time() - check_time + check_time = time.time() # evaluate the collision shape defined in an object's template # 1. get the template from MM matching_templates = sim.get_object_template_manager().get_template_handles( @@ -333,8 +354,8 @@ def evaluate_collision_shape( ) obj_template.compute_COM_from_shape = False obj_template.com = mn.Vector3(0) - obj_template.bounding_box_collisions = True - obj_template.is_collidable = False + # obj_template.bounding_box_collisions = True + # obj_template.is_collidable = False sim.get_object_template_manager().register_template(obj_template) # 2. Setup a stage from the object's render mesh stm = sim.get_stage_template_manager() @@ -349,20 +370,40 @@ def evaluate_collision_shape( new_settings["scene"] = stage_template_name new_config = make_cfg(new_settings) sim.reconfigure(new_config) + profile_metrics["init_stage"] = time.time() - check_time + check_time = time.time() + # 4. compute initial metric baselines scene_bb = sim.get_active_scene_graph().get_root_node().cumulative_bb inflated_scene_bb = scene_bb.scaled(mn.Vector3(1.25)) inflated_scene_bb = mn.Range3D.from_center( scene_bb.center(), inflated_scene_bb.size() / 2.0 ) - # bounding box sample - # test_points = sample_points_from_range3d(range3d=inflated_scene_bb) - # bounding sphere sample - half_diagonal = (scene_bb.max - scene_bb.min).length() / 2.0 - test_points = sample_points_from_sphere( - center=inflated_scene_bb.center(), radius=half_diagonal - ) + test_points = None + if sample_shape == "aabb": + # bounding box sample + test_points = sample_points_from_range3d( + range3d=inflated_scene_bb, num_points=num_point_samples + ) + elif sample_shape == "sphere": + # bounding sphere sample + half_diagonal = (scene_bb.max - scene_bb.min).length() / 2.0 + test_points = sample_points_from_sphere( + center=inflated_scene_bb.center(), + radius=half_diagonal, + num_points=num_point_samples, + ) + else: + raise NotImplementedError( + f"sample_shape == `{sample_shape}` is not implemented. Use `sphere` or `aabb`." + ) + profile_metrics["sample_points"] = time.time() - check_time + check_time = time.time() + gt_raycast_results = run_pairwise_raycasts(test_points, sim) + profile_metrics["raycast_stage"] = time.time() - check_time + check_time = time.time() + # 5. load the object with proxy (in NONE stage) new_settings = sim_settings.copy() new_config = make_cfg(new_settings) @@ -371,18 +412,29 @@ def evaluate_collision_shape( obj_template_handle ) obj.translation = obj.com + profile_metrics["init_object"] = time.time() - check_time + check_time = time.time() + # 6. compute the metric for proxy object pr_raycast_results = run_pairwise_raycasts(test_points, sim) + profile_metrics["raycast_object"] = time.time() - check_time + check_time = time.time() + # 7. compare metrics normalized_error = get_raycast_results_cumulative_error_metric( gt_raycast_results, pr_raycast_results ) + profile_metrics["compute_metrics"] = time.time() - check_time + check_time = time.time() + profile_metrics["total"] = time.time() - start_time + return ( gt_raycast_results, pr_raycast_results, obj_template_handle, test_points, normalized_error, + profile_metrics, ) From 1b62fa5633b2c00a301a5e8b018dc61f2de07b32 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 6 Mar 2023 16:09:34 -0800 Subject: [PATCH 003/153] add jittered sampling --- examples/viewer.py | 9 +- tools/collision_shape_automation.py | 123 +++++++++++++++++++++++----- 2 files changed, 106 insertions(+), 26 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index fcd98d287b..ec1e3cc71f 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1329,11 +1329,11 @@ def next_frame() -> None: mm = habitat_sim.metadata.MetadataMediator() mm.active_dataset = sim_settings["scene_dataset_config_file"] - num_point_samples = 1000 + num_point_samples = 100 sample_metrics = [] avg_metric = 0 avg_profile_metrics = {} - for _sample in range(5): + for _sample in range(10): ( gt_raycast_results, pr_raycast_results, @@ -1344,7 +1344,7 @@ def next_frame() -> None: ) = csa.evaluate_collision_shape( obj_name, sim_settings, - sample_shape="aabb", + sample_shape="jittered_aabb", mm=mm, num_point_samples=num_point_samples, ) @@ -1364,8 +1364,9 @@ def next_frame() -> None: variance = 0 for metric in sample_metrics: variance += (metric - avg_metric) ** 2 - variance /= len(sample_metrics) + variance /= len(sample_metrics) - 1 print(f"variance = {variance}") + print(f"std = {math.sqrt(variance)}") print(f"avg_profile_metrics = {avg_profile_metrics}") exit() diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 52f4de6537..3994b1902e 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -3,6 +3,7 @@ # LICENSE file in the root directory of this source tree. import argparse +import math import random import time from typing import Any, Dict, List, Tuple @@ -17,6 +18,9 @@ # chair - good approximation: 0a5e809804911e71de6a4ef89f2c8fef5b9291b3.glb # shelves - bad approximation: d1d1e0cdaba797ee70882e63f66055675c3f1e7f.glb +# ======================================================================= +# Range3D surface sampling utils + def compute_area_weights_for_range3d_faces(range3d: mn.Range3D): """ @@ -41,29 +45,10 @@ def compute_area_weights_for_range3d_faces(range3d: mn.Range3D): return normalized_area_accumulator -def sample_points_from_range3d( - range3d: mn.Range3D, num_points: int = 100 -) -> List[List[mn.Vector3]]: +def get_range3d_sample_planes(range3d: mn.Range3D): """ - Sample 'num_points' from the surface of a box defeined by 'range3d'. + Get origin and basis vectors for each face's sample planes. """ - - # ----------------------------------------- - # area weighted face sampling - normalized_area_accumulator = compute_area_weights_for_range3d_faces(range3d) - - def sample_face() -> int: - """ - Weighted sampling of a face from the area accumulator. - """ - rand = random.random() - for ix in range(6): - if normalized_area_accumulator[ix] > rand: - return ix - raise (AssertionError, "Should not reach here.") - - # ----------------------------------------- - # For each face a starting point and two edge vectors (un-normalized) face_info: List[Tuple[mn.Vector3, mn.Vector3, mn.Vector3]] = [ ( @@ -97,6 +82,91 @@ def sample_face() -> int: mn.Vector3.z_axis(range3d.size_z()), ), # right ] + return face_info + + +def sample_jittered_points_from_range3d(range3d: mn.Range3D, num_points: int = 100): + """ + Use jittered sampling to compute a more uniformly distributed set of random points. + """ + normalized_area_accumulator = compute_area_weights_for_range3d_faces(range3d) + normalized_areas = [] + for vix in range(len(normalized_area_accumulator)): + if vix == 0: + normalized_areas.append(normalized_area_accumulator[vix]) + else: + normalized_areas.append( + normalized_area_accumulator[vix] - normalized_area_accumulator[vix - 1] + ) + + # get number of points per face based on area + # NOTE: rounded up, so may be slightly more points than requested. + points_per_face = [max(1, math.ceil(x * num_points)) for x in normalized_areas] + + # get face plane basis + face_info = get_range3d_sample_planes(range3d) + + # one internal list of each face of the box: + samples = [] + for _ in range(6): + samples.append([]) + + real_total_points = 0 + # print("Sampling Stats: ") + # for each face, jittered sample of total area: + for face_ix, f in enumerate(face_info): + # get ratio of width/height in local space to plan jittering + aspect_ratio = f[1].length() / f[2].length() + num_wide = max(1, int(math.sqrt(aspect_ratio * points_per_face[face_ix]))) + num_high = max(1, int((points_per_face[face_ix] + num_wide - 1) / num_wide)) + total_points = num_wide * num_high + real_total_points += total_points + # print(f" f_{face_ix}: ") + # print(f" points_per_face = {points_per_face[face_ix]}") + # print(f" aspect_ratio = {aspect_ratio}") + # print(f" num_wide = {num_wide}") + # print(f" num_high = {num_high}") + # print(f" total_points = {total_points}") + + # get jittered cell sizes + dx = f[1] / num_wide + dy = f[2] / num_high + for x in range(num_wide): + for y in range(num_high): + # get cell origin + org = f[0] + x * dx + y * dy + # point is randomly placed in the cell + point = org + random.random() * dx + random.random() * dy + samples[face_ix].append(point) + # print(f" real_total_points = {real_total_points}") + + return samples + + +def sample_points_from_range3d( + range3d: mn.Range3D, num_points: int = 100 +) -> List[List[mn.Vector3]]: + """ + Sample 'num_points' from the surface of a box defeined by 'range3d'. + """ + + # ----------------------------------------- + # area weighted face sampling + normalized_area_accumulator = compute_area_weights_for_range3d_faces(range3d) + + def sample_face() -> int: + """ + Weighted sampling of a face from the area accumulator. + """ + rand = random.random() + for ix in range(6): + if normalized_area_accumulator[ix] > rand: + return ix + raise (AssertionError, "Should not reach here.") + + # ----------------------------------------- + + face_info = get_range3d_sample_planes(range3d) # one internal list of each face of the box: samples = [] @@ -113,6 +183,10 @@ def sample_face() -> int: return samples +# End - Range3D surface sampling utils +# ======================================================================= + + def sample_points_from_sphere( center: mn.Vector3, radius: float, num_points: int = 100 ) -> List[List[mn.Vector3]]: @@ -328,7 +402,7 @@ def evaluate_collision_shape( :param object_handle: The object to evaluate. :param sim_settings: Any simulator settings for initialization (should be physics enabled and point to correct dataset). - :param sample_shape: The desired bounding shape for raycast: "sphere" or "aabb". + :param sample_shape: The desired bounding shape for raycast: "sphere" or "aabb", "jittered_aabb". :param mm: A pre-configured MetadataMediator may be provided to reduce initialization time. Should have the correct dataset already configured. """ profile_metrics = {} @@ -385,6 +459,11 @@ def evaluate_collision_shape( test_points = sample_points_from_range3d( range3d=inflated_scene_bb, num_points=num_point_samples ) + elif sample_shape == "jittered_aabb": + # bounding box sample + test_points = sample_jittered_points_from_range3d( + range3d=inflated_scene_bb, num_points=num_point_samples + ) elif sample_shape == "sphere": # bounding sphere sample half_diagonal = (scene_bb.max - scene_bb.min).length() / 2.0 From 820072e569cfc38f29aa513abaec82305e8fcea8 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 7 Mar 2023 11:07:01 -0800 Subject: [PATCH 004/153] Implement stateful API class for easier iteration and export of batch results --- tools/collision_shape_automation.py | 334 +++++++++++++++++++++++++++- 1 file changed, 330 insertions(+), 4 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 3994b1902e..d932103aea 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -517,20 +517,346 @@ def evaluate_collision_shape( ) +# =================================================================== +# CollisionProxyOptimizer class provides a stateful API for +# configurable evaluation and optimization of collision proxy shapes. +# =================================================================== + + +class CollisionProxyOptimizer: + """ + Stateful control flow for using Habitat-sim to evaluate and optimize collision proxy shapes. + """ + + def __init__(self, sim_settings: Dict[str, Any]) -> None: + # load the dataset into a persistent, shared MetadataMediator instance. + self.mm = habitat_sim.metadata.MetadataMediator() + self.mm.active_dataset = sim_settings["scene_dataset_config_file"] + + # cache of test points, rays, distances, etc... for use by active processes + # NOTE: entries created by `setup_obj_gt` and cleaned by `clean_obj_gt` for memory efficiency. + self.gt_data: Dict[str, Dict[str, Any]] = {} + + # cache global results to be written to csv. + self.results: Dict[str, Dict[str, Any]] = {} + + def get_cfg_with_mm( + self, scene: str = "NONE" + ) -> habitat_sim.simulator.Configuration: + """ + Get a Configuration object for initializing habitat_sim Simulator object with the correct dataset and MetadataMediator passed along. + + :param scene: The desired scene entry, defaulting to the empty NONE scene. + """ + sim_settings = default_sim_settings.copy() + sim_settings["scene_dataset_config_file"] = self.mm.active_dataset + sim_settings["scene"] = scene + cfg = make_cfg(sim_settings) + cfg.metadata_mediator = self.mm + return cfg + + def setup_obj_gt( + self, + obj_handle: str, + sample_shape: str = "jittered_aabb", + num_point_samples=100, + ) -> None: + """ + Prepare the ground truth and sample point sets for an object. + """ + assert ( + obj_handle not in self.gt_data + ), f"`{obj_handle}` already setup in gt_data: {self.gt_data.keys()}" + + # find object + otm = self.mm.object_template_manager + obj_template = otm.get_template_by_handle(obj_handle) + assert obj_template is not None, f"Could not find object handle `{obj_handle}`" + # correct now for any COM automation + obj_template.compute_COM_from_shape = False + obj_template.com = mn.Vector3(0) + otm.register_template(obj_template) + + self.gt_data[obj_handle] = {} + + # load a simulator instance with this object as the stage + stm = self.mm.stage_template_manager + stage_template_name = obj_handle + "_as_stage" + self.gt_data[obj_handle]["stage_template_name"] = stage_template_name + new_stage_template = stm.create_new_template(handle=stage_template_name) + new_stage_template.render_asset_handle = obj_template.render_asset_handle + stm.register_template( + template=new_stage_template, specified_handle=stage_template_name + ) + cfg = self.get_cfg_with_mm(scene=stage_template_name) + with habitat_sim.Simulator(cfg) as sim: + # get test points from bounding box info: + scene_bb = sim.get_active_scene_graph().get_root_node().cumulative_bb + inflated_scene_bb = scene_bb.scaled(mn.Vector3(1.25)) + inflated_scene_bb = mn.Range3D.from_center( + scene_bb.center(), inflated_scene_bb.size() / 2.0 + ) + test_points = None + if sample_shape == "aabb": + # bounding box sample + test_points = sample_points_from_range3d( + range3d=inflated_scene_bb, num_points=num_point_samples + ) + elif sample_shape == "jittered_aabb": + # bounding box sample + test_points = sample_jittered_points_from_range3d( + range3d=inflated_scene_bb, num_points=num_point_samples + ) + elif sample_shape == "sphere": + # bounding sphere sample + half_diagonal = (scene_bb.max - scene_bb.min).length() / 2.0 + test_points = sample_points_from_sphere( + center=inflated_scene_bb.center(), + radius=half_diagonal, + num_points=num_point_samples, + ) + else: + raise NotImplementedError( + f"sample_shape == `{sample_shape}` is not implemented. Use `sphere` or `aabb`." + ) + self.gt_data[obj_handle]["test_points"] = test_points + + # compute and cache "ground truth" raycast on object as stage + gt_raycast_results = run_pairwise_raycasts(test_points, sim) + self.gt_data[obj_handle]["raycasts"] = { + "gt": {"results": gt_raycast_results} + } + + def clean_obj_gt(self, obj_handle: str) -> None: + """ + Cleans the global object cache to better manage process memory. + Call this to clean-up after global data are written and detailed sample data are no longer necessary. + """ + assert ( + obj_handle in self.gt_data + ), f"`{obj_handle}` does not have any entry in gt_data: {self.gt_data.keys()}. Call to `setup_obj_gt(obj_handle)` required." + self.gt_data.pop(obj_handle) + + def compute_baseline_metrics(self, obj_handle: str) -> None: + """ + Computes 2 baselines for the evaluation metric and caches the results: + 1. No collision object + 2. AABB collision object + """ + assert ( + obj_handle in self.gt_data + ), f"`{obj_handle}` does not have any entry in gt_data: {self.gt_data.keys()}. Call to `setup_obj_gt(obj_handle)` required." + + print(self.gt_data[obj_handle]["raycasts"].keys()) + + # start with empty scene + cfg = self.get_cfg_with_mm() + with habitat_sim.Simulator(cfg) as sim: + empty_raycast_results = run_pairwise_raycasts( + self.gt_data[obj_handle]["test_points"], sim + ) + self.gt_data[obj_handle]["raycasts"]["empty"] = { + "results": empty_raycast_results + } + + cfg = self.get_cfg_with_mm() + with habitat_sim.Simulator(cfg) as sim: + # modify the template + obj_template = sim.get_object_template_manager().get_template_by_handle( + obj_handle + ) + assert ( + obj_template is not None + ), f"Could not find object handle `{obj_handle}`" + # bounding box as collision object + obj_template.bounding_box_collisions = True + sim.get_object_template_manager().register_template(obj_template) + + # load the object + sim.get_rigid_object_manager().add_object_by_template_handle(obj_handle) + + # run evaluation + bb_raycast_results = run_pairwise_raycasts( + self.gt_data[obj_handle]["test_points"], sim + ) + self.gt_data[obj_handle]["raycasts"]["bb"] = {"results": bb_raycast_results} + + # un-modify the template + obj_template.bounding_box_collisions = False + sim.get_object_template_manager().register_template(obj_template) + + def compute_proxy_metrics(self, obj_handle: str) -> None: + """ + Computes the evaluation metric on the currently configred proxy shape and caches the results. + """ + assert ( + obj_handle in self.gt_data + ), f"`{obj_handle}` does not have any entry in gt_data: {self.gt_data.keys()}. Call to `setup_obj_gt(obj_handle)` required." + + # start with empty scene + cfg = self.get_cfg_with_mm() + with habitat_sim.Simulator(cfg) as sim: + # load the object + sim.get_rigid_object_manager().add_object_by_template_handle(obj_handle) + + # run evaluation + pr_raycast_results = run_pairwise_raycasts( + self.gt_data[obj_handle]["test_points"], sim + ) + self.gt_data[obj_handle]["raycasts"]["pr"] = {"results": pr_raycast_results} + + def compute_gt_errors(self, obj_handle: str) -> None: + """ + Compute and cache all ground truth error metrics. + Assumes `self.gt_data[obj_handle]["raycasts"]` keys are different raycast results to be compared. + 'gt' must exist. + """ + + assert ( + obj_handle in self.gt_data + ), f"`{obj_handle}` does not have any entry in gt_data: {self.gt_data.keys()}. Call to `setup_obj_gt(obj_handle)` required." + assert ( + len(self.gt_data[obj_handle]["raycasts"]) > 1 + ), "Only gt results acquired, no error to compute. Try `compute_proxy_metrics` or `compute_baseline_metrics`." + assert ( + "gt" in self.gt_data[obj_handle]["raycasts"] + ), "Must have a ground truth to compare against. Should be generated in `setup_obj_gt(obj_handle)`." + + for key in self.gt_data[obj_handle]["raycasts"].keys(): + if ( + key != "gt" + and "normalized_errors" not in self.gt_data[obj_handle]["raycasts"][key] + ): + normalized_error = get_raycast_results_cumulative_error_metric( + self.gt_data[obj_handle]["raycasts"]["gt"]["results"], + self.gt_data[obj_handle]["raycasts"][key]["results"], + ) + self.gt_data[obj_handle]["raycasts"][key][ + "normalized_errors" + ] = normalized_error + + def cache_global_results(self) -> None: + """ + Cache the current global cumulative results. + Do this after an object's computation is done before cleaning the gt data. + """ + + for obj_handle in self.gt_data.keys(): + if obj_handle not in self.results: + self.results[obj_handle] = {} + for key in self.gt_data[obj_handle]["raycasts"].keys(): + if ( + key != "gt" + and "normalized_errors" in self.gt_data[obj_handle]["raycasts"][key] + ): + if "normalized_errors" not in self.results[obj_handle]: + self.results[obj_handle]["normalized_errors"] = {} + self.results[obj_handle]["normalized_errors"][key] = self.gt_data[ + obj_handle + ]["raycasts"][key]["normalized_errors"] + + def save_results_to_csv(self, filename: str) -> None: + """ + Save current global results to a csv file. + """ + + assert len(self.results) > 0, "There musst be results to save." + + import csv + + # save normalized error csv + with open(filename, "w") as f: + writer = csv.writer(f, quoting=csv.QUOTE_ALL) + # first collect all column names: + existing_cols = [] + for obj_handle in self.results.keys(): + if "normalized_errors" in self.results[obj_handle]: + for key in self.results[obj_handle]["normalized_errors"]: + if key not in existing_cols: + existing_cols.append(key) + print(f"existing_cols = {existing_cols}") + # put the baselines first + ordered_cols = ["object_handle"] + if "empty" in existing_cols: + ordered_cols.append("empty") + if "bb" in existing_cols: + ordered_cols.append("bb") + for key in existing_cols: + if key not in ordered_cols: + ordered_cols.append(key) + print(f"ordered_cols = {ordered_cols}") + # write column names row + writer.writerow(ordered_cols) + + # write results rows + for obj_handle in self.results.keys(): + row_data = [obj_handle] + if "normalized_errors" in self.results[obj_handle]: + for key in ordered_cols: + if key != "object_handle": + if key in self.results[obj_handle]["normalized_errors"]: + row_data.append( + self.results[obj_handle]["normalized_errors"][key] + ) + else: + row_data.append("") + print(f"row_data = {row_data}") + writer.writerow(row_data) + + def compute_and_save_results_for_objects( + self, obj_handle_substrings: List[str], output_filename: str = "cpo_out.csv" + ) -> None: + # first find all full object handles + otm = self.mm.object_template_manager + obj_handles = [] + for obj_h in obj_handle_substrings: + # find the full handle + matching_obj_handles = otm.get_file_template_handles(obj_h) + assert ( + len(matching_obj_handles) == 1 + ), f"None or many matching handles to substring `{obj_h}`: {matching_obj_handles}" + obj_handles.append(matching_obj_handles[0]) + + print(f"Found handles: {obj_handles}.") + print("Computing metrics:") + # then compute metrics for all objects and cache + for obix, obj_h in enumerate(obj_handles): + print("-------------------------------") + print(f"Computing metric for `{obj_h}`, {obix}|{len(obj_handles)}") + print("-------------------------------") + self.setup_obj_gt(obj_h) + self.compute_baseline_metrics(obj_h) + self.compute_proxy_metrics(obj_h) + self.compute_gt_errors(obj_h) + self.cache_global_results() + self.clean_obj_gt(obj_h) + + # then save results to file + self.save_results_to_csv(output_filename) + + def main(): parser = argparse.ArgumentParser( description="Automate collision shape creation and validation." ) parser.add_argument("--dataset", type=str, help="path to SceneDataset.") - parser.add_argument( - "--object-handle", type=str, help="handle identifying the object to evaluate." - ) + # parser.add_argument( + # "--object-handle", type=str, help="handle identifying the object to evaluate." + # ) args = parser.parse_args() sim_settings = default_sim_settings.copy() sim_settings["scene_dataset_config_file"] = args.dataset - evaluate_collision_shape(args.object_handle, sim_settings) + # use the CollisionProxyOptimizer to compute metrics for multiple objects + cpo = CollisionProxyOptimizer(sim_settings) + + obj_handle1 = "0a5e809804911e71de6a4ef89f2c8fef5b9291b3" + obj_handle2 = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" + + cpo.compute_and_save_results_for_objects([obj_handle1, obj_handle2]) + + # evaluate_collision_shape(args.object_handle, sim_settings) if __name__ == "__main__": From 2e62aa65efc5b37c9abd53443af4d90845a35e6e Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 7 Mar 2023 15:00:23 -0800 Subject: [PATCH 005/153] add check and solution for invalid raycasts (hits outside bounding shape) and some QoL features for viewer --- examples/viewer.py | 46 +++++++++++++----------- tools/collision_shape_automation.py | 55 +++++++++++++++++++++-------- 2 files changed, 67 insertions(+), 34 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index ec1e3cc71f..274879197e 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -181,6 +181,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.sample_seed = 0 self.collision_proxy_obj = None self.mouse_cast_results = None + self.debug_draw_raycasts = True # toggle a single simulation step at the next opportunity if not # simulating continuously. @@ -277,13 +278,26 @@ def debug_draw(self): ) self.draw_region_debug(debug_line_render) - if gt_raycast_results is not None: + # mouse raycast circle + white = mn.Color4(mn.Vector3(1.0), 1.0) + if self.mouse_cast_results is not None and self.mouse_cast_results.has_hits(): + m_ray = self.mouse_cast_results.ray + self.sim.get_debug_line_render().draw_circle( + translation=m_ray.origin + + m_ray.direction + * self.mouse_cast_results.hits[0].ray_distance + * m_ray.direction.length(), + radius=0.005, + color=white, + normal=self.mouse_cast_results.hits[0].normal, + ) + + if gt_raycast_results is not None and self.debug_draw_raycasts: scene_bb = self.sim.get_active_scene_graph().get_root_node().cumulative_bb inflated_scene_bb = scene_bb.scaled(mn.Vector3(1.25)) inflated_scene_bb = mn.Range3D.from_center( scene_bb.center(), inflated_scene_bb.size() / 2.0 ) - white = mn.Color4(mn.Vector3(1.0), 1.0) self.sim.get_debug_line_render().draw_box( inflated_scene_bb.min, inflated_scene_bb.max, white ) @@ -304,24 +318,9 @@ def debug_draw(self): self.sim.get_debug_line_render().draw_circle( translation=p, radius=0.005, - color=mn.Color4.yellow(), + color=mn.Color4.magenta(), ) - if ( - self.mouse_cast_results is not None - and self.mouse_cast_results.has_hits() - ): - m_ray = self.mouse_cast_results.ray - self.sim.get_debug_line_render().draw_circle( - translation=m_ray.origin - + m_ray.direction - * self.mouse_cast_results.hits[0].ray_distance - * m_ray.direction.length(), - radius=0.005, - color=white, - normal=self.mouse_cast_results.hits[0].normal, - ) - def draw_event( self, simulation_call: Optional[Callable] = None, @@ -663,7 +662,10 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.collision_proxy_obj.translation = mn.Vector3(0) elif key == pressed.T: - if shift_pressed: + if alt_pressed: + self.debug_draw_raycasts = not self.debug_draw_raycasts + print(f"Toggled self.debug_draw_raycasts: {self.debug_draw_raycasts}") + elif shift_pressed: self.sample_seed -= 1 else: self.sample_seed += 1 @@ -1325,6 +1327,10 @@ def next_frame() -> None: obj_name = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" + # check against default + if args.scene != "./data/test_assets/scenes/simple_room.glb": + obj_name = args.scene + # load JSON once instead of repeating mm = habitat_sim.metadata.MetadataMediator() mm.active_dataset = sim_settings["scene_dataset_config_file"] @@ -1368,7 +1374,7 @@ def next_frame() -> None: print(f"variance = {variance}") print(f"std = {math.sqrt(variance)}") print(f"avg_profile_metrics = {avg_profile_metrics}") - exit() + # exit() # start the application HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index d932103aea..c01a8e0ad6 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -216,13 +216,22 @@ def sample_points_from_sphere( def run_pairwise_raycasts( - points: List[List[mn.Vector3]], sim: habitat_sim.Simulator, min_dist: float = 0.1 + points: List[List[mn.Vector3]], + sim: habitat_sim.Simulator, + min_dist: float = 0.05, + discard_invalid_results: bool = True, ) -> List[habitat_sim.physics.RaycastResults]: """ Raycast between each pair of points from different surfaces. :param min_dist: The minimum ray distance to allow. Cull all candidate pairs closer than this distance. + :param discard_invalid_results: If true, discard ray hit distances > 1 """ + ray_max_local_dist = 100.0 # default + if discard_invalid_results: + # disallow contacts outside of the bounding volume (shouldn't happen anyway...) + ray_max_local_dist = 1.0 all_raycast_results: List[habitat_sim.physics.RaycastResults] = [] + print("Rays detected with invalid hit distance: ") for fix0 in range(len(points)): for fix1 in range(len(points)): if fix0 != fix1: # no pairs on the same face @@ -232,11 +241,25 @@ def run_pairwise_raycasts( # this is a valid pair of points ray = habitat_sim.geo.Ray(p0, p1 - p0) # origin, direction # raycast - all_raycast_results.append(sim.cast_ray(ray=ray)) + all_raycast_results.append( + sim.cast_ray(ray=ray, max_distance=ray_max_local_dist) + ) # reverse direction as separate entry (because exiting a convex does not generate a hit record) ray2 = habitat_sim.geo.Ray(p1, p0 - p1) # origin, direction # raycast - all_raycast_results.append(sim.cast_ray(ray=ray2)) + all_raycast_results.append( + sim.cast_ray(ray=ray2, max_distance=ray_max_local_dist) + ) + + # prints invalid rays if not discarded by discard_invalid_results==True + for ix in [-1, -2]: + if all_raycast_results[ix].has_hits() and ( + all_raycast_results[ix].hits[0].ray_distance > 1 + or all_raycast_results[ix].hits[0].ray_distance < 0 + ): + print( + f" distance={all_raycast_results[ix].hits[0].ray_distance}" + ) return all_raycast_results @@ -265,8 +288,8 @@ def debug_draw_raycast_results( overshoot_dists = [] undershoot_dists = [] + # draw first hits for gt and proxy if gt_results.has_hits(): - # draw first and last hits for gt and proxy sim.get_debug_line_render().draw_circle( translation=ray.origin + ray.direction * gt_results.hits[0].ray_distance, @@ -479,6 +502,7 @@ def evaluate_collision_shape( profile_metrics["sample_points"] = time.time() - check_time check_time = time.time() + print("GT raycast:") gt_raycast_results = run_pairwise_raycasts(test_points, sim) profile_metrics["raycast_stage"] = time.time() - check_time check_time = time.time() @@ -495,6 +519,7 @@ def evaluate_collision_shape( check_time = time.time() # 6. compute the metric for proxy object + print("PR raycast:") pr_raycast_results = run_pairwise_raycasts(test_points, sim) profile_metrics["raycast_object"] = time.time() - check_time check_time = time.time() @@ -647,8 +672,6 @@ def compute_baseline_metrics(self, obj_handle: str) -> None: obj_handle in self.gt_data ), f"`{obj_handle}` does not have any entry in gt_data: {self.gt_data.keys()}. Call to `setup_obj_gt(obj_handle)` required." - print(self.gt_data[obj_handle]["raycasts"].keys()) - # start with empty scene cfg = self.get_cfg_with_mm() with habitat_sim.Simulator(cfg) as sim: @@ -774,7 +797,6 @@ def save_results_to_csv(self, filename: str) -> None: for key in self.results[obj_handle]["normalized_errors"]: if key not in existing_cols: existing_cols.append(key) - print(f"existing_cols = {existing_cols}") # put the baselines first ordered_cols = ["object_handle"] if "empty" in existing_cols: @@ -784,7 +806,6 @@ def save_results_to_csv(self, filename: str) -> None: for key in existing_cols: if key not in ordered_cols: ordered_cols.append(key) - print(f"ordered_cols = {ordered_cols}") # write column names row writer.writerow(ordered_cols) @@ -800,7 +821,6 @@ def save_results_to_csv(self, filename: str) -> None: ) else: row_data.append("") - print(f"row_data = {row_data}") writer.writerow(row_data) def compute_and_save_results_for_objects( @@ -848,15 +868,22 @@ def main(): sim_settings = default_sim_settings.copy() sim_settings["scene_dataset_config_file"] = args.dataset + # one-off single object logic: + # evaluate_collision_shape(args.object_handle, sim_settings) + # use the CollisionProxyOptimizer to compute metrics for multiple objects cpo = CollisionProxyOptimizer(sim_settings) - obj_handle1 = "0a5e809804911e71de6a4ef89f2c8fef5b9291b3" - obj_handle2 = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" + # get all object handles + all_handles = cpo.mm.object_template_manager.get_file_template_handles() + # get a subset for scale testing + all_handles = all_handles[:100] + cpo.compute_and_save_results_for_objects(all_handles) - cpo.compute_and_save_results_for_objects([obj_handle1, obj_handle2]) - - # evaluate_collision_shape(args.object_handle, sim_settings) + # testing objects + # obj_handle1 = "0a5e809804911e71de6a4ef89f2c8fef5b9291b3" + # obj_handle2 = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" + # cpo.compute_and_save_results_for_objects([obj_handle1, obj_handle2]) if __name__ == "__main__": From 93ef8e2d6cfa259b8c14f480908d39eda7f90274 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 7 Mar 2023 16:22:26 -0800 Subject: [PATCH 006/153] don't use navmesh in object viewer --- examples/viewer.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 274879197e..8e2f4b2231 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -196,12 +196,15 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.reconfigure_sim() self.debug_semantic_colors = {} + if self.sim.pathfinder.is_loaded: + self.sim.pathfinder = habitat_sim.nav.PathFinder() + # compute NavMesh if not already loaded by the scene. - if ( - not self.sim.pathfinder.is_loaded - and self.cfg.sim_cfg.scene_id.lower() != "none" - ): - self.navmesh_config_and_recompute() + # if ( + # not self.sim.pathfinder.is_loaded + # and self.cfg.sim_cfg.scene_id.lower() != "none" + # ): + # self.navmesh_config_and_recompute() self.time_since_last_simulation = 0.0 LoggingContext.reinitialize_from_env() From 8a59dfadf2592364bc83f28bcd4a4fe0fb808a30 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 7 Mar 2023 16:24:24 -0800 Subject: [PATCH 007/153] only evaluate objects with receptacles defined --- tools/collision_shape_automation.py | 34 +++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index c01a8e0ad6..e42d745a9c 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -855,6 +855,29 @@ def compute_and_save_results_for_objects( self.save_results_to_csv(output_filename) +def object_has_receptacles( + object_template_handle: str, + otm: habitat_sim.attributes_managers.ObjectAttributesManager, +) -> bool: + """ + Returns whether or not an object has a receptacle defined in its config file. + """ + # this prefix will be present for any entry which defines a receptacle + receptacle_prefix_string = "receptacle_" + + object_template = otm.get_template_by_handle(object_template_handle) + assert ( + object_template is not None + ), f"No template matching handle {object_template_handle}." + + user_cfg = object_template.get_user_config() + + return any( + sub_config_key.startswith(receptacle_prefix_string) + for sub_config_key in user_cfg.get_subconfig_keys() + ) + + def main(): parser = argparse.ArgumentParser( description="Automate collision shape creation and validation." @@ -875,8 +898,15 @@ def main(): cpo = CollisionProxyOptimizer(sim_settings) # get all object handles - all_handles = cpo.mm.object_template_manager.get_file_template_handles() - # get a subset for scale testing + otm = cpo.mm.object_template_manager + all_handles = otm.get_file_template_handles() + # get a subset with receptacles defined + all_handles = [ + all_handles[i] + for i in range(len(all_handles)) + if object_has_receptacles(all_handles[i], otm) + ] + print(f"Number of objects with receptacles = {len(all_handles)}") all_handles = all_handles[:100] cpo.compute_and_save_results_for_objects(all_handles) From df61c68cee091760aa53df34a26069f92ed18ff6 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 7 Mar 2023 18:13:44 -0800 Subject: [PATCH 008/153] add debug peek images for ground truth and proxy shapes. Add specified output directory. Import Habitat-lab features. --- tools/collision_shape_automation.py | 78 ++++++++++++++++++++++++++--- 1 file changed, 72 insertions(+), 6 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index e42d745a9c..95c0eab0e9 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -4,10 +4,15 @@ import argparse import math +import os import random import time from typing import Any, Dict, List, Tuple +# imports from Habitat-lab +# NOTE: requires PR 1108 branch: rearrange-gen-improvements (https://github.com/facebookresearch/habitat-lab/pull/1108) +# import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle +import habitat.sims.habitat_simulator.debug_visualizer as hab_debug_vis import magnum as mn import numpy as np @@ -553,10 +558,18 @@ class CollisionProxyOptimizer: Stateful control flow for using Habitat-sim to evaluate and optimize collision proxy shapes. """ - def __init__(self, sim_settings: Dict[str, Any]) -> None: + def __init__(self, sim_settings: Dict[str, Any], output_directory="") -> None: # load the dataset into a persistent, shared MetadataMediator instance. self.mm = habitat_sim.metadata.MetadataMediator() self.mm.active_dataset = sim_settings["scene_dataset_config_file"] + self.sim_settings = sim_settings.copy() + + # path to the desired output directory for images/csv + self.output_directory = output_directory + os.makedirs(self.output_directory, exist_ok=True) + + # if true, render and save debug images in self.output_directory + self.generate_debug_images = False # cache of test points, rays, distances, etc... for use by active processes # NOTE: entries created by `setup_obj_gt` and cleaned by `clean_obj_gt` for memory efficiency. @@ -573,7 +586,7 @@ def get_cfg_with_mm( :param scene: The desired scene entry, defaulting to the empty NONE scene. """ - sim_settings = default_sim_settings.copy() + sim_settings = self.sim_settings.copy() sim_settings["scene_dataset_config_file"] = self.mm.active_dataset sim_settings["scene"] = scene cfg = make_cfg(sim_settings) @@ -597,6 +610,43 @@ def setup_obj_gt( otm = self.mm.object_template_manager obj_template = otm.get_template_by_handle(obj_handle) assert obj_template is not None, f"Could not find object handle `{obj_handle}`" + + # capture debug images of the ground truth object and collision asset before adjusting COM + if self.generate_debug_images: + cfg = self.get_cfg_with_mm() + with habitat_sim.Simulator(cfg) as sim: + # load the gt object + rom = sim.get_rigid_object_manager() + obj = rom.add_object_by_template_handle(obj_handle) + assert obj.is_alive, "Object was not added correctly." + # use DebugVisualizer to get 6-axis view of the object + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + dvb.peek_rigid_object( + obj, peek_all_axis=True, additional_savefile_prefix="gt_" + ) + # modify the template to render collision object + render_asset = obj_template.render_asset_handle + obj_template.render_asset_handle = obj_template.collision_asset_handle + otm.register_template(obj_template) + col_obj = rom.add_object_by_template_handle(obj_handle) + # use DebugVisualizer to get 6-axis view of both objects + dvb.peek_rigid_object( + obj, peek_all_axis=True, additional_savefile_prefix="combined_" + ) + # remove ground truth object + rom.remove_object_by_handle(obj.handle) + # use DebugVisualizer to get 6-axis view of the collision object + dvb.peek_rigid_object( + col_obj, peek_all_axis=True, additional_savefile_prefix="pr_" + ) + # undo template modification + obj_template.render_asset_handle = render_asset + otm.register_template(obj_template) + # correct now for any COM automation obj_template.compute_COM_from_shape = False obj_template.com = mn.Vector3(0) @@ -720,7 +770,10 @@ def compute_proxy_metrics(self, obj_handle: str) -> None: cfg = self.get_cfg_with_mm() with habitat_sim.Simulator(cfg) as sim: # load the object - sim.get_rigid_object_manager().add_object_by_template_handle(obj_handle) + obj = sim.get_rigid_object_manager().add_object_by_template_handle( + obj_handle + ) + assert obj.is_alive, "Object was not added correctly." # run evaluation pr_raycast_results = run_pairwise_raycasts( @@ -780,15 +833,17 @@ def cache_global_results(self) -> None: def save_results_to_csv(self, filename: str) -> None: """ - Save current global results to a csv file. + Save current global results to a csv file in the self.output_directory. """ assert len(self.results) > 0, "There musst be results to save." import csv + filepath = os.path.join(self.output_directory, filename) + # save normalized error csv - with open(filename, "w") as f: + with open(filepath, "w") as f: writer = csv.writer(f, quoting=csv.QUOTE_ALL) # first collect all column names: existing_cols = [] @@ -883,6 +938,12 @@ def main(): description="Automate collision shape creation and validation." ) parser.add_argument("--dataset", type=str, help="path to SceneDataset.") + parser.add_argument( + "--output-dir", + type=str, + default="collision_shape_automation/", + help="output directory for saved csv and images. Default = `./collision_shape_automation/`.", + ) # parser.add_argument( # "--object-handle", type=str, help="handle identifying the object to evaluate." # ) @@ -890,12 +951,17 @@ def main(): sim_settings = default_sim_settings.copy() sim_settings["scene_dataset_config_file"] = args.dataset + # necessary for debug rendering + sim_settings["sensor_height"] = 0 + sim_settings["width"] = 720 + sim_settings["height"] = 720 # one-off single object logic: # evaluate_collision_shape(args.object_handle, sim_settings) # use the CollisionProxyOptimizer to compute metrics for multiple objects - cpo = CollisionProxyOptimizer(sim_settings) + cpo = CollisionProxyOptimizer(sim_settings, output_directory=args.output_dir) + cpo.generate_debug_images = True # get all object handles otm = cpo.mm.object_template_manager From e1b0be2b79d58c58015eab9f5e57308f99344dbc Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 9 Mar 2023 13:53:28 -0800 Subject: [PATCH 009/153] refactor to better modularize debug image capture suring proxy phase. Validate proxy bounding box is similar to ground truth. Prepare for Recetpacle sampling TODO. --- tools/collision_shape_automation.py | 106 +++++++++++++++++++--------- 1 file changed, 71 insertions(+), 35 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 95c0eab0e9..210de4a8d6 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -11,7 +11,7 @@ # imports from Habitat-lab # NOTE: requires PR 1108 branch: rearrange-gen-improvements (https://github.com/facebookresearch/habitat-lab/pull/1108) -# import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle +import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle import habitat.sims.habitat_simulator.debug_visualizer as hab_debug_vis import magnum as mn import numpy as np @@ -571,6 +571,9 @@ def __init__(self, sim_settings: Dict[str, Any], output_directory="") -> None: # if true, render and save debug images in self.output_directory self.generate_debug_images = False + # option to use Receptacle annotations to compute an additional accuracy metric + self.compute_receptacle_useability_metrics = True + # cache of test points, rays, distances, etc... for use by active processes # NOTE: entries created by `setup_obj_gt` and cleaned by `clean_obj_gt` for memory efficiency. self.gt_data: Dict[str, Dict[str, Any]] = {} @@ -611,48 +614,49 @@ def setup_obj_gt( obj_template = otm.get_template_by_handle(obj_handle) assert obj_template is not None, f"Could not find object handle `{obj_handle}`" - # capture debug images of the ground truth object and collision asset before adjusting COM - if self.generate_debug_images: + self.gt_data[obj_handle] = {} + + # correct now for any COM automation + obj_template.compute_COM_from_shape = False + obj_template.com = mn.Vector3(0) + otm.register_template(obj_template) + + if self.compute_receptacle_useability_metrics or self.generate_debug_images: + # pre-process the ground truth object and receptacles cfg = self.get_cfg_with_mm() with habitat_sim.Simulator(cfg) as sim: # load the gt object rom = sim.get_rigid_object_manager() obj = rom.add_object_by_template_handle(obj_handle) assert obj.is_alive, "Object was not added correctly." - # use DebugVisualizer to get 6-axis view of the object - dvb = hab_debug_vis.DebugVisualizer( - sim=sim, - output_path=self.output_directory, - default_sensor_uuid="color_sensor", - ) - dvb.peek_rigid_object( - obj, peek_all_axis=True, additional_savefile_prefix="gt_" - ) - # modify the template to render collision object - render_asset = obj_template.render_asset_handle - obj_template.render_asset_handle = obj_template.collision_asset_handle - otm.register_template(obj_template) - col_obj = rom.add_object_by_template_handle(obj_handle) - # use DebugVisualizer to get 6-axis view of both objects - dvb.peek_rigid_object( - obj, peek_all_axis=True, additional_savefile_prefix="combined_" - ) - # remove ground truth object - rom.remove_object_by_handle(obj.handle) - # use DebugVisualizer to get 6-axis view of the collision object - dvb.peek_rigid_object( - col_obj, peek_all_axis=True, additional_savefile_prefix="pr_" - ) - # undo template modification - obj_template.render_asset_handle = render_asset - otm.register_template(obj_template) - # correct now for any COM automation - obj_template.compute_COM_from_shape = False - obj_template.com = mn.Vector3(0) - otm.register_template(obj_template) + if self.compute_receptacle_useability_metrics: + # get receptacles defined for the object: + source_template_file = obj.creation_attributes.file_directory + user_attr = obj.user_attributes + obj_receptacles = hab_receptacle.parse_receptacles_from_user_config( + user_attr, + parent_object_handle=obj_handle, + parent_template_directory=source_template_file, + ) - self.gt_data[obj_handle] = {} + # sample test points on the receptacles + self.gt_data[obj_handle]["receptacles"] = {} + for _receptacle in obj_receptacles: + # TODO: sample receptacle points + # test_points = receptacle.sample_uniform_global() + pass + + if self.generate_debug_images: + # use DebugVisualizer to get 6-axis view of the object + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + dvb.peek_rigid_object( + obj, peek_all_axis=True, additional_savefile_prefix="gt_" + ) # load a simulator instance with this object as the stage stm = self.mm.stage_template_manager @@ -671,6 +675,8 @@ def setup_obj_gt( inflated_scene_bb = mn.Range3D.from_center( scene_bb.center(), inflated_scene_bb.size() / 2.0 ) + self.gt_data[obj_handle]["scene_bb"] = scene_bb + self.gt_data[obj_handle]["inflated_scene_bb"] = inflated_scene_bb test_points = None if sample_shape == "aabb": # bounding box sample @@ -769,18 +775,48 @@ def compute_proxy_metrics(self, obj_handle: str) -> None: # start with empty scene cfg = self.get_cfg_with_mm() with habitat_sim.Simulator(cfg) as sim: + # modify the template to render collision object + otm = self.mm.object_template_manager + obj_template = otm.get_template_by_handle(obj_handle) + render_asset = obj_template.render_asset_handle + obj_template.render_asset_handle = obj_template.collision_asset_handle + otm.register_template(obj_template) + # load the object obj = sim.get_rigid_object_manager().add_object_by_template_handle( obj_handle ) assert obj.is_alive, "Object was not added correctly." + # check that collision shape bounding box is similar + col_bb = obj.root_scene_node.cumulative_bb + assert self.gt_data[obj_handle]["inflated_scene_bb"].contains( + col_bb.min + ) and self.gt_data[obj_handle]["inflated_scene_bb"].contains( + col_bb.max + ), f"Inflated bounding box does not contain the collision shape. (Object `{obj_handle}`)" + + if self.generate_debug_images: + # use DebugVisualizer to get 6-axis view of the object + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + dvb.peek_rigid_object( + obj, peek_all_axis=True, additional_savefile_prefix="pr_" + ) + # run evaluation pr_raycast_results = run_pairwise_raycasts( self.gt_data[obj_handle]["test_points"], sim ) self.gt_data[obj_handle]["raycasts"]["pr"] = {"results": pr_raycast_results} + # undo template modification + obj_template.render_asset_handle = render_asset + otm.register_template(obj_template) + def compute_gt_errors(self, obj_handle: str) -> None: """ Compute and cache all ground truth error metrics. From 2fbcb93063212da3fe6d69c9cc8c757c98f789f2 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 9 Mar 2023 14:33:33 -0800 Subject: [PATCH 010/153] receptacle sampling and caching --- tools/collision_shape_automation.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 210de4a8d6..e7f5eccb05 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -642,10 +642,13 @@ def setup_obj_gt( # sample test points on the receptacles self.gt_data[obj_handle]["receptacles"] = {} - for _receptacle in obj_receptacles: - # TODO: sample receptacle points - # test_points = receptacle.sample_uniform_global() - pass + for receptacle in obj_receptacles: + rec_test_points = [] + for _ in range(num_point_samples): + rec_test_points.append(receptacle.sample_uniform_global()) + self.gt_data[obj_handle]["receptacles"][ + receptacle.name + ] = rec_test_points if self.generate_debug_images: # use DebugVisualizer to get 6-axis view of the object From ea4e1657ebc2742f65e64a892e5de43f88d7a2f3 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 10 Mar 2023 08:19:44 -0800 Subject: [PATCH 011/153] magenta/alpha clear color for better object contrast. Add Receptacle debug image draw. --- tools/collision_shape_automation.py | 55 +++++++++++++++++++++++++---- 1 file changed, 48 insertions(+), 7 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index e7f5eccb05..68ff0fed4f 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -636,19 +636,59 @@ def setup_obj_gt( user_attr = obj.user_attributes obj_receptacles = hab_receptacle.parse_receptacles_from_user_config( user_attr, - parent_object_handle=obj_handle, + parent_object_handle=obj.handle, parent_template_directory=source_template_file, ) # sample test points on the receptacles self.gt_data[obj_handle]["receptacles"] = {} for receptacle in obj_receptacles: - rec_test_points = [] - for _ in range(num_point_samples): - rec_test_points.append(receptacle.sample_uniform_global()) - self.gt_data[obj_handle]["receptacles"][ - receptacle.name - ] = rec_test_points + if type(receptacle) == hab_receptacle.TriangleMeshReceptacle: + rec_test_points = [] + for _ in range(num_point_samples): + rec_test_points.append( + receptacle.sample_uniform_global( + sim, sample_region_scale=1.0 + ) + ) + self.gt_data[obj_handle]["receptacles"][ + receptacle.name + ] = rec_test_points + if self.generate_debug_images: + debug_lines = [] + assert ( + len(receptacle.mesh_data[1]) % 3 == 0 + ), "must be triangles" + for face in range( + int(len(receptacle.mesh_data[1]) / 3) + ): + verts = receptacle.get_face_verts(f_ix=face) + for edge in range(3): + debug_lines.append( + ( + [verts[edge], verts[(edge + 1) % 3]], + mn.Color4.green(), + ) + ) + for p in rec_test_points: + debug_lines.append( + ( + [p, p + mn.Vector3(0, 0.01, 0)], + mn.Color4.red(), + ) + ) + # use DebugVisualizer to get 6-axis view of the object + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + dvb.peek_rigid_object( + obj, + peek_all_axis=True, + additional_savefile_prefix=f"{receptacle.name}_", + debug_lines=debug_lines, + ) if self.generate_debug_images: # use DebugVisualizer to get 6-axis view of the object @@ -994,6 +1034,7 @@ def main(): sim_settings["sensor_height"] = 0 sim_settings["width"] = 720 sim_settings["height"] = 720 + sim_settings["clear_color"] = mn.Color4.magenta() * 0.5 # one-off single object logic: # evaluate_collision_shape(args.object_handle, sim_settings) From 2a82a24413da953aca4d5a5fcb8d2ecef40fd227 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 10 Mar 2023 18:37:32 -0800 Subject: [PATCH 012/153] Added: receptacle access metrics v1, receptacle density sampling, hemisphere raycasting, debug draw improvements. --- tools/collision_shape_automation.py | 287 ++++++++++++++++++++++++++-- 1 file changed, 270 insertions(+), 17 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 68ff0fed4f..02f3dba3b8 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -23,6 +23,88 @@ # chair - good approximation: 0a5e809804911e71de6a4ef89f2c8fef5b9291b3.glb # shelves - bad approximation: d1d1e0cdaba797ee70882e63f66055675c3f1e7f.glb +# 71 equidistant points on a unit hemisphere generated from icosphere subdivision +# Sphere center is (0,0,0) and no points lie on x,z plane +# used for hemisphere raycasting from Receptacle points +icosphere_points_subdiv_3 = [ + mn.Vector3(-0.276388, 0.447220, -0.850649), + mn.Vector3(-0.483971, 0.502302, -0.716565), + mn.Vector3(-0.232822, 0.657519, -0.716563), + mn.Vector3(0.723607, 0.447220, -0.525725), + mn.Vector3(0.531941, 0.502302, -0.681712), + mn.Vector3(0.609547, 0.657519, -0.442856), + mn.Vector3(0.723607, 0.447220, 0.525725), + mn.Vector3(0.812729, 0.502301, 0.295238), + mn.Vector3(0.609547, 0.657519, 0.442856), + mn.Vector3(-0.276388, 0.447220, 0.850649), + mn.Vector3(-0.029639, 0.502302, 0.864184), + mn.Vector3(-0.232822, 0.657519, 0.716563), + mn.Vector3(-0.894426, 0.447216, 0.000000), + mn.Vector3(-0.831051, 0.502299, 0.238853), + mn.Vector3(-0.753442, 0.657515, 0.000000), + mn.Vector3(-0.251147, 0.967949, 0.000000), + mn.Vector3(-0.077607, 0.967950, 0.238853), + mn.Vector3(0.000000, 1.000000, 0.000000), + mn.Vector3(-0.525730, 0.850652, 0.000000), + mn.Vector3(-0.361800, 0.894429, 0.262863), + mn.Vector3(-0.638194, 0.723610, 0.262864), + mn.Vector3(-0.162456, 0.850654, 0.499995), + mn.Vector3(-0.447209, 0.723612, 0.525728), + mn.Vector3(-0.688189, 0.525736, 0.499997), + mn.Vector3(-0.483971, 0.502302, 0.716565), + mn.Vector3(0.203181, 0.967950, 0.147618), + mn.Vector3(0.138197, 0.894430, 0.425319), + mn.Vector3(0.052790, 0.723612, 0.688185), + mn.Vector3(0.425323, 0.850654, 0.309011), + mn.Vector3(0.361804, 0.723612, 0.587778), + mn.Vector3(0.262869, 0.525738, 0.809012), + mn.Vector3(0.531941, 0.502302, 0.681712), + mn.Vector3(0.203181, 0.967950, -0.147618), + mn.Vector3(0.447210, 0.894429, 0.000000), + mn.Vector3(0.670817, 0.723611, 0.162457), + mn.Vector3(0.425323, 0.850654, -0.309011), + mn.Vector3(0.670817, 0.723611, -0.162457), + mn.Vector3(0.850648, 0.525736, 0.000000), + mn.Vector3(0.812729, 0.502301, -0.295238), + mn.Vector3(-0.077607, 0.967950, -0.238853), + mn.Vector3(0.138197, 0.894430, -0.425319), + mn.Vector3(0.361804, 0.723612, -0.587778), + mn.Vector3(-0.162456, 0.850654, -0.499995), + mn.Vector3(0.052790, 0.723612, -0.688185), + mn.Vector3(0.262869, 0.525738, -0.809012), + mn.Vector3(-0.029639, 0.502302, -0.864184), + mn.Vector3(-0.361800, 0.894429, -0.262863), + mn.Vector3(-0.447209, 0.723612, -0.525728), + mn.Vector3(-0.638194, 0.723610, -0.262864), + mn.Vector3(-0.688189, 0.525736, -0.499997), + mn.Vector3(-0.831051, 0.502299, -0.238853), + mn.Vector3(-0.956626, 0.251149, 0.147618), + mn.Vector3(-0.861804, 0.276396, 0.425322), + mn.Vector3(-0.670821, 0.276397, 0.688189), + mn.Vector3(-0.436007, 0.251152, 0.864188), + mn.Vector3(-0.155215, 0.251152, 0.955422), + mn.Vector3(0.138199, 0.276397, 0.951055), + mn.Vector3(0.447215, 0.276397, 0.850649), + mn.Vector3(0.687159, 0.251152, 0.681715), + mn.Vector3(0.860698, 0.251151, 0.442858), + mn.Vector3(0.947213, 0.276396, 0.162458), + mn.Vector3(0.947213, 0.276397, -0.162458), + mn.Vector3(0.860698, 0.251151, -0.442858), + mn.Vector3(0.687159, 0.251152, -0.681715), + mn.Vector3(0.447216, 0.276397, -0.850648), + mn.Vector3(0.138199, 0.276397, -0.951055), + mn.Vector3(-0.155215, 0.251152, -0.955422), + mn.Vector3(-0.436007, 0.251152, -0.864188), + mn.Vector3(-0.670820, 0.276396, -0.688190), + mn.Vector3(-0.861804, 0.276394, -0.425323), + mn.Vector3(-0.956626, 0.251149, -0.147618), +] + + +def get_scaled_hemisphere_vectors(scale: float): + return [v * scale for v in icosphere_points_subdiv_3] + + # ======================================================================= # Range3D surface sampling utils @@ -193,11 +275,16 @@ def sample_face() -> int: def sample_points_from_sphere( - center: mn.Vector3, radius: float, num_points: int = 100 + center: mn.Vector3, + radius: float, + num_points: int = 100, ) -> List[List[mn.Vector3]]: """ Sample num_points from a sphere defined by center and radius. Return all points in two identical lists to indicate pairwise raycasting. + :param center: sphere center position + :param radius: sphere radius + :param num_points: number of points to sample """ samples = [] @@ -220,6 +307,47 @@ def sample_points_from_sphere( return samples +def receptacle_density_sample( + sim: habitat_sim.simulator.Simulator, + receptacle: hab_receptacle.TriangleMeshReceptacle, + target_radius: float = 0.04, + max_points: int = 100, + min_points: int = 5, + max_tries: int = 200, +): + target_point_area = math.pi * target_radius**2 + expected_points = receptacle.total_area / target_point_area + + # if necessary, compute new target_radius to best cover the area + if expected_points > max_points or expected_points < min_points: + expected_points = max(min_points, min(max_points, expected_points)) + target_radius = math.sqrt(receptacle.total_area / (expected_points * math.pi)) + + # print(f"receptacle_density_sample(`{receptacle.name}`): area={receptacle.total_area}, r={target_radius}, num_p={expected_points}") + + sampled_points = [] + num_tries = 0 + min_dist = target_radius * 2 + while len(sampled_points) < expected_points and num_tries < max_tries: + sample_point = receptacle.sample_uniform_global(sim, sample_region_scale=1.0) + success = True + for existing_point in sampled_points: + if (sample_point - existing_point).length() < min_dist: + num_tries += 1 + success = False + break + if success: + # print(f" success {sample_point} in {num_tries} tries") + + # if no rejection, add the point + sampled_points.append(sample_point) + num_tries = 0 + + # print(f" found {len(sampled_points)}/{expected_points} points.") + + return sampled_points, target_radius + + def run_pairwise_raycasts( points: List[List[mn.Vector3]], sim: habitat_sim.Simulator, @@ -645,15 +773,21 @@ def setup_obj_gt( for receptacle in obj_receptacles: if type(receptacle) == hab_receptacle.TriangleMeshReceptacle: rec_test_points = [] - for _ in range(num_point_samples): - rec_test_points.append( - receptacle.sample_uniform_global( - sim, sample_region_scale=1.0 - ) - ) - self.gt_data[obj_handle]["receptacles"][ - receptacle.name - ] = rec_test_points + t_radius = 0.01 + # adaptive density sample: + rec_test_points, t_radius = receptacle_density_sample( + sim, receptacle + ) + # random sample: + # for _ in range(num_point_samples): + # rec_test_points.append( + # receptacle.sample_uniform_global( + # sim, sample_region_scale=1.0 + # ) + # ) + self.gt_data[obj_handle]["receptacles"][receptacle.name] = { + "sample_points": rec_test_points + } if self.generate_debug_images: debug_lines = [] assert ( @@ -670,11 +804,16 @@ def setup_obj_gt( mn.Color4.green(), ) ) + debug_circles = [] for p in rec_test_points: - debug_lines.append( + debug_circles.append( ( - [p, p + mn.Vector3(0, 0.01, 0)], - mn.Color4.red(), + ( + p, # center + t_radius, # radius + mn.Vector3(0, 1, 0), # normal + mn.Color4.red(), # color + ) ) ) # use DebugVisualizer to get 6-axis view of the object @@ -688,6 +827,7 @@ def setup_obj_gt( peek_all_axis=True, additional_savefile_prefix=f"{receptacle.name}_", debug_lines=debug_lines, + debug_circles=debug_circles, ) if self.generate_debug_images: @@ -718,7 +858,10 @@ def setup_obj_gt( inflated_scene_bb = mn.Range3D.from_center( scene_bb.center(), inflated_scene_bb.size() / 2.0 ) - self.gt_data[obj_handle]["scene_bb"] = scene_bb + # NOTE: to save the referenced Range3D object, we need to deep or Magnum will destroy the underlying C++ objects. + self.gt_data[obj_handle]["scene_bb"] = mn.Range3D( + scene_bb.min, scene_bb.max + ) self.gt_data[obj_handle]["inflated_scene_bb"] = inflated_scene_bb test_points = None if sample_shape == "aabb": @@ -815,6 +958,13 @@ def compute_proxy_metrics(self, obj_handle: str) -> None: obj_handle in self.gt_data ), f"`{obj_handle}` does not have any entry in gt_data: {self.gt_data.keys()}. Call to `setup_obj_gt(obj_handle)` required." + # when evaluating multiple proxy shapes, need unique ids: + pr_id = "pr0" + id_counter = 0 + while pr_id in self.gt_data[obj_handle]["raycasts"]: + pr_id = "pr" + str(id_counter) + id_counter += 1 + # start with empty scene cfg = self.get_cfg_with_mm() with habitat_sim.Simulator(cfg) as sim: @@ -847,19 +997,115 @@ def compute_proxy_metrics(self, obj_handle: str) -> None: default_sensor_uuid="color_sensor", ) dvb.peek_rigid_object( - obj, peek_all_axis=True, additional_savefile_prefix="pr_" + obj, peek_all_axis=True, additional_savefile_prefix=pr_id + "_" ) # run evaluation pr_raycast_results = run_pairwise_raycasts( self.gt_data[obj_handle]["test_points"], sim ) - self.gt_data[obj_handle]["raycasts"]["pr"] = {"results": pr_raycast_results} + self.gt_data[obj_handle]["raycasts"][pr_id] = { + "results": pr_raycast_results + } # undo template modification obj_template.render_asset_handle = render_asset otm.register_template(obj_template) + def compute_receptacle_access_metrics( + self, obj_handle: str, use_gt=False, acces_ratio_threshold: float = 0.1 + ): + """ + Compute a heuristic for the accessibility of all Receptacles for an object. + Uses raycasting from previously sampled receptacle locations to approximate how open a particular receptacle is. + :param use_gt: Compute the metric for the ground truth shape instead of the currently active collision proxy (default) + :param acces_ratio_threshold: The ratio of accessible:blocked rays necessary for a recetpacle point to be considered accessible + """ + # algorithm: + # For each receptacle, r: + # For each sample point, s: + # Generate `num_point_rays` directions, d (length bb diagnonal) and Ray(origin=s+d, direction=d) + # For each ray: + # If dist > 1, success, otherwise failure + # + # metrics: + # - %rays + # - %points w/ success% > eps(10%) #call these successful/accessible + # - average % for points + # ? how to get regions? + # ? debug draw this metric? + # ? how to diff b/t gt and pr? + + print(f"compute_receptacle_access_metrics - obj_handle = {obj_handle}") + + # start with empty scene or stage as scene: + scene_name = "NONE" + if use_gt: + scene_name = self.gt_data[obj_handle]["stage_template_name"] + cfg = self.get_cfg_with_mm(scene=scene_name) + with habitat_sim.Simulator(cfg) as sim: + if not use_gt: + # load the object + obj = sim.get_rigid_object_manager().add_object_by_template_handle( + obj_handle + ) + assert obj.is_alive, "Object was not added correctly." + + # gather hemisphere rays scaled to object's size + # NOTE: because the receptacle points can be located anywhere in the bounding box, raycast radius must be bb diagonal length + ray_sphere_radius = self.gt_data[obj_handle]["scene_bb"].size().length() + assert ray_sphere_radius > 0, "otherwise we have an error" + ray_sphere_points = get_scaled_hemisphere_vectors(ray_sphere_radius) + + # collect hemisphere raycast samples for all receptacle sample points + obj_rec_data = self.gt_data[obj_handle]["receptacles"] + for receptacle_name in obj_rec_data.keys(): + sample_point_ray_results: List[ + List[habitat_sim.physics.RaycastResults] + ] = [] + sample_point_access_ratios: List[float] = [] + # access rate is percent of "accessible" points apssing the threshold + receptacle_access_rate = 0 + # access score is average accessibility of points + receptacle_access_score = 0 + sample_points = obj_rec_data[receptacle_name]["sample_points"] + for sample_point in sample_points: + # NOTE: rays must originate outside the shape because origins inside a convex will not collide. + # move ray origins to new point location + hemi_rays = [ + habitat_sim.geo.Ray(v + sample_point, -v) + for v in ray_sphere_points + ] + # rays are not unit length, so use local max_distance==1 ray length + ray_results = [ + sim.cast_ray(ray=ray, max_distance=1.0) for ray in hemi_rays + ] + sample_point_ray_results.append(ray_results) + + # compute per-point access metrics + blocked_rays = len([rr for rr in ray_results if rr.has_hits()]) + sample_point_access_ratios.append( + (len(ray_results) - blocked_rays) / len(ray_results) + ) + receptacle_access_score += sample_point_access_ratios[-1] + if sample_point_access_ratios[-1] > acces_ratio_threshold: + receptacle_access_rate += 1 + + receptacle_access_score /= len(sample_points) + receptacle_access_rate /= len(sample_points) + obj_rec_data[receptacle_name][ + "sample_point_ray_results" + ] = sample_point_ray_results + obj_rec_data[receptacle_name][ + "receptacle_access_score" + ] = receptacle_access_score + obj_rec_data[receptacle_name][ + "receptacle_access_rate" + ] = receptacle_access_rate + print(f" receptacle_name = {receptacle_name}") + print(f" receptacle_access_score = {receptacle_access_score}") + print(f" receptacle_access_rate = {receptacle_access_rate}") + def compute_gt_errors(self, obj_handle: str) -> None: """ Compute and cache all ground truth error metrics. @@ -893,7 +1139,7 @@ def compute_gt_errors(self, obj_handle: str) -> None: def cache_global_results(self) -> None: """ Cache the current global cumulative results. - Do this after an object's computation is done before cleaning the gt data. + Do this after an object's computation is done (compute_gt_errors) before cleaning the gt data. """ for obj_handle in self.gt_data.keys(): @@ -909,6 +1155,7 @@ def cache_global_results(self) -> None: self.results[obj_handle]["normalized_errors"][key] = self.gt_data[ obj_handle ]["raycasts"][key]["normalized_errors"] + # TODO: cache the receptacle access metrics for CSV save def save_results_to_csv(self, filename: str) -> None: """ @@ -981,6 +1228,12 @@ def compute_and_save_results_for_objects( self.setup_obj_gt(obj_h) self.compute_baseline_metrics(obj_h) self.compute_proxy_metrics(obj_h) + # receptacle metrics: + if self.compute_receptacle_useability_metrics: + print(" GT Recetpacle Metrics:") + self.compute_receptacle_access_metrics(obj_h, use_gt=True) + print(" PR Recetpacle Metrics:") + self.compute_receptacle_access_metrics(obj_h, use_gt=False) self.compute_gt_errors(obj_h) self.cache_global_results() self.clean_obj_gt(obj_h) From 1c25396626e33958ff374b654df1c05587eb4efe Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 13 Mar 2023 11:53:23 -0700 Subject: [PATCH 013/153] debug visualization of receptacle access metrics and receptacle vertical offset option --- tools/collision_shape_automation.py | 115 +++++++++++++++++++++++++++- 1 file changed, 111 insertions(+), 4 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 02f3dba3b8..200bd1c557 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -701,6 +701,8 @@ def __init__(self, sim_settings: Dict[str, Any], output_directory="") -> None: # option to use Receptacle annotations to compute an additional accuracy metric self.compute_receptacle_useability_metrics = True + # add a vertical epsilon offset to the receptacle points for analysis. This is added directly to the sampled points. + self.rec_point_vertical_offset = 0.02 # cache of test points, rays, distances, etc... for use by active processes # NOTE: entries created by `setup_obj_gt` and cleaned by `clean_obj_gt` for memory efficiency. @@ -751,6 +753,7 @@ def setup_obj_gt( if self.compute_receptacle_useability_metrics or self.generate_debug_images: # pre-process the ground truth object and receptacles + rec_vertical_offset = mn.Vector3(0, self.rec_point_vertical_offset, 0) cfg = self.get_cfg_with_mm() with habitat_sim.Simulator(cfg) as sim: # load the gt object @@ -778,6 +781,11 @@ def setup_obj_gt( rec_test_points, t_radius = receptacle_density_sample( sim, receptacle ) + # add the vertical offset + rec_test_points = [ + p + rec_vertical_offset for p in rec_test_points + ] + # random sample: # for _ in range(num_point_samples): # rec_test_points.append( @@ -1044,6 +1052,9 @@ def compute_receptacle_access_metrics( scene_name = self.gt_data[obj_handle]["stage_template_name"] cfg = self.get_cfg_with_mm(scene=scene_name) with habitat_sim.Simulator(cfg) as sim: + obj_rec_data = self.gt_data[obj_handle]["receptacles"] + shape_id = "gt" + obj = None if not use_gt: # load the object obj = sim.get_rigid_object_manager().add_object_by_template_handle( @@ -1051,15 +1062,36 @@ def compute_receptacle_access_metrics( ) assert obj.is_alive, "Object was not added correctly." + # when evaluating multiple proxy shapes, need unique ids: + pr_id = "pr0" + id_counter = 0 + while pr_id in self.gt_data[obj_handle]["raycasts"]: + pr_id = "pr" + str(id_counter) + id_counter += 1 + shape_id = pr_id + # gather hemisphere rays scaled to object's size # NOTE: because the receptacle points can be located anywhere in the bounding box, raycast radius must be bb diagonal length ray_sphere_radius = self.gt_data[obj_handle]["scene_bb"].size().length() assert ray_sphere_radius > 0, "otherwise we have an error" ray_sphere_points = get_scaled_hemisphere_vectors(ray_sphere_radius) + # save a list of point accessibility scores for debugging and visualization + receptacle_point_access_scores = {} + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + # collect hemisphere raycast samples for all receptacle sample points - obj_rec_data = self.gt_data[obj_handle]["receptacles"] for receptacle_name in obj_rec_data.keys(): + if "results" not in obj_rec_data[receptacle_name]: + obj_rec_data[receptacle_name]["results"] = {} + assert ( + shape_id not in obj_rec_data[receptacle_name]["results"] + ), f" overwriting results for {shape_id}" + obj_rec_data[receptacle_name]["results"][shape_id] = {} sample_point_ray_results: List[ List[habitat_sim.physics.RaycastResults] ] = [] @@ -1090,22 +1122,97 @@ def compute_receptacle_access_metrics( receptacle_access_score += sample_point_access_ratios[-1] if sample_point_access_ratios[-1] > acces_ratio_threshold: receptacle_access_rate += 1 + receptacle_point_access_scores[ + receptacle_name + ] = sample_point_access_ratios receptacle_access_score /= len(sample_points) receptacle_access_rate /= len(sample_points) - obj_rec_data[receptacle_name][ + + obj_rec_data[receptacle_name]["results"][shape_id][ "sample_point_ray_results" ] = sample_point_ray_results - obj_rec_data[receptacle_name][ + obj_rec_data[receptacle_name]["results"][shape_id][ "receptacle_access_score" ] = receptacle_access_score - obj_rec_data[receptacle_name][ + obj_rec_data[receptacle_name]["results"][shape_id][ "receptacle_access_rate" ] = receptacle_access_rate print(f" receptacle_name = {receptacle_name}") print(f" receptacle_access_score = {receptacle_access_score}") print(f" receptacle_access_rate = {receptacle_access_rate}") + if self.generate_debug_images: + # generate receptacle access debug images + # 1a Show missed rays vs 1b hit rays + debug_lines = [] + for ray_results in obj_rec_data[receptacle_name]["results"][ + shape_id + ]["sample_point_ray_results"]: + for hit_record in ray_results: + if not hit_record.has_hits(): + debug_lines.append( + ( + [ + hit_record.ray.origin, + hit_record.ray.origin + + hit_record.ray.direction, + ], + mn.Color4.green(), + ) + ) + if use_gt: + dvb.peek_scene( + peek_all_axis=True, + additional_savefile_prefix=f"{receptacle_name}_access_rays_", + debug_lines=debug_lines, + debug_circles=None, + ) + else: + dvb.peek_rigid_object( + obj, + peek_all_axis=True, + additional_savefile_prefix=f"{receptacle_name}_access_rays_", + debug_lines=debug_lines, + debug_circles=None, + ) + + # 2 Show only rec points colored by "access" metric or percentage + debug_circles = [] + color_r = mn.Color4.red().to_xyz() + color_g = mn.Color4.green().to_xyz() + delta = color_g - color_r + for point_access_ratio, point in zip( + receptacle_point_access_scores[receptacle_name], + obj_rec_data[receptacle_name]["sample_points"], + ): + point_color_xyz = color_r + delta * point_access_ratio + debug_circles.append( + ( + point, + 0.02, + mn.Vector3(0, 1, 0), + mn.Color4.from_xyz(point_color_xyz), + ) + ) + # use DebugVisualizer to get 6-axis view of the object + if use_gt: + dvb.peek_scene( + peek_all_axis=True, + additional_savefile_prefix=f"{receptacle_name}_point_ratios_", + debug_lines=None, + debug_circles=debug_circles, + ) + else: + dvb.peek_rigid_object( + obj, + peek_all_axis=True, + additional_savefile_prefix=f"{receptacle_name}_point_ratios_", + debug_lines=None, + debug_circles=debug_circles, + ) + # obj_rec_data[receptacle_name]["results"][shape_id]["sample_point_ray_results"] + def compute_gt_errors(self, obj_handle: str) -> None: """ Compute and cache all ground truth error metrics. From f1e009fe2c579541ca95cc0dca435f9c4a423062 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 13 Mar 2023 14:51:36 -0700 Subject: [PATCH 014/153] implement saving receptacle access metrics to CSV --- tools/collision_shape_automation.py | 90 ++++++++++++++++++++++++++--- 1 file changed, 83 insertions(+), 7 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 200bd1c557..e75237a4cb 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1063,11 +1063,14 @@ def compute_receptacle_access_metrics( assert obj.is_alive, "Object was not added correctly." # when evaluating multiple proxy shapes, need unique ids: - pr_id = "pr0" + pr_id = None + next_pr_id = "pr0" id_counter = 0 - while pr_id in self.gt_data[obj_handle]["raycasts"]: - pr_id = "pr" + str(id_counter) + while next_pr_id in self.gt_data[obj_handle]["raycasts"]: + pr_id = next_pr_id + next_pr_id = "pr" + str(id_counter) id_counter += 1 + assert pr_id is not None shape_id = pr_id # gather hemisphere rays scaled to object's size @@ -1262,21 +1265,48 @@ def cache_global_results(self) -> None: self.results[obj_handle]["normalized_errors"][key] = self.gt_data[ obj_handle ]["raycasts"][key]["normalized_errors"] - # TODO: cache the receptacle access metrics for CSV save + + if self.compute_receptacle_useability_metrics: + self.results[obj_handle]["receptacle_info"] = {} + # cache the receptacle access metrics for CSV save + for rec_key in self.gt_data[obj_handle]["receptacles"].keys(): + self.results[obj_handle]["receptacle_info"][rec_key] = {} + assert ( + "results" in self.gt_data[obj_handle]["receptacles"][rec_key] + ), "Must run 'compute_receptacle_access_metrics' before caching global receptacle data." + rec_results = self.gt_data[obj_handle]["receptacles"][rec_key][ + "results" + ] + + # access rate and score + for metric in ["receptacle_access_score", "receptacle_access_rate"]: + for shape_id in rec_results.keys(): + if ( + metric + not in self.results[obj_handle]["receptacle_info"][ + rec_key + ] + ): + self.results[obj_handle]["receptacle_info"][rec_key][ + metric + ] = {} + self.results[obj_handle]["receptacle_info"][rec_key][ + metric + ][shape_id] = rec_results[shape_id][metric] def save_results_to_csv(self, filename: str) -> None: """ Save current global results to a csv file in the self.output_directory. """ - assert len(self.results) > 0, "There musst be results to save." + assert len(self.results) > 0, "There must be results to save." import csv filepath = os.path.join(self.output_directory, filename) # save normalized error csv - with open(filepath, "w") as f: + with open(filepath + ".csv", "w") as f: writer = csv.writer(f, quoting=csv.QUOTE_ALL) # first collect all column names: existing_cols = [] @@ -1311,8 +1341,54 @@ def save_results_to_csv(self, filename: str) -> None: row_data.append("") writer.writerow(row_data) + # export receptacle metrics to CSV + if self.compute_receptacle_useability_metrics: + rec_filepath = filepath + "_receptacle_metrics" + with open(rec_filepath + ".csv", "w") as f: + writer = csv.writer(f, quoting=csv.QUOTE_ALL) + # first collect all column names: + existing_cols = ["receptacle"] + shape_ids = [] + metrics = [] + for obj_handle in self.results.keys(): + if "receptacle_info" in self.results[obj_handle]: + for rec_key in self.results[obj_handle]["receptacle_info"]: + for metric in self.results[obj_handle]["receptacle_info"][ + rec_key + ].keys(): + if metric not in metrics: + metrics.append(metric) + for shape_id in self.results[obj_handle][ + "receptacle_info" + ][rec_key][metric].keys(): + if shape_id not in shape_ids: + shape_ids.append(shape_id) + if shape_id + "-" + metric not in existing_cols: + existing_cols.append(shape_id + "-" + metric) + # write column names row + writer.writerow(existing_cols) + + # write results rows + for obj_handle in self.results.keys(): + if "receptacle_info" in self.results[obj_handle]: + for rec_key in self.results[obj_handle]["receptacle_info"]: + rec_info = self.results[obj_handle]["receptacle_info"][ + rec_key + ] + row_data = [obj_handle + "_" + rec_key] + for metric in metrics: + for shape_id in shape_ids: + if ( + metric in rec_info + and shape_id in rec_info[metric] + ): + row_data.append(rec_info[metric][shape_id]) + else: + row_data.append("") + writer.writerow(row_data) + def compute_and_save_results_for_objects( - self, obj_handle_substrings: List[str], output_filename: str = "cpo_out.csv" + self, obj_handle_substrings: List[str], output_filename: str = "cpo_out" ) -> None: # first find all full object handles otm = self.mm.object_template_manager From 1fb3cbe268b9b0f710a4d1b01c87fefbf88323a8 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 13 Mar 2023 17:30:28 -0700 Subject: [PATCH 015/153] prototype VHACD optimization loop. Minor bugfixes. --- tools/collision_shape_automation.py | 69 +++++++++++++++++++++++++++-- 1 file changed, 65 insertions(+), 4 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index e75237a4cb..0e99c4db0f 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1167,7 +1167,7 @@ def compute_receptacle_access_metrics( if use_gt: dvb.peek_scene( peek_all_axis=True, - additional_savefile_prefix=f"{receptacle_name}_access_rays_", + additional_savefile_prefix=f"gt_{receptacle_name}_access_rays_", debug_lines=debug_lines, debug_circles=None, ) @@ -1175,7 +1175,7 @@ def compute_receptacle_access_metrics( dvb.peek_rigid_object( obj, peek_all_axis=True, - additional_savefile_prefix=f"{receptacle_name}_access_rays_", + additional_savefile_prefix=f"{shape_id}_{receptacle_name}_access_rays_", debug_lines=debug_lines, debug_circles=None, ) @@ -1202,7 +1202,7 @@ def compute_receptacle_access_metrics( if use_gt: dvb.peek_scene( peek_all_axis=True, - additional_savefile_prefix=f"{receptacle_name}_point_ratios_", + additional_savefile_prefix=f"gt_{receptacle_name}_point_ratios_", debug_lines=None, debug_circles=debug_circles, ) @@ -1210,7 +1210,7 @@ def compute_receptacle_access_metrics( dvb.peek_rigid_object( obj, peek_all_axis=True, - additional_savefile_prefix=f"{receptacle_name}_point_ratios_", + additional_savefile_prefix=f"{shape_id}_{receptacle_name}_point_ratios_", debug_lines=None, debug_circles=debug_circles, ) @@ -1246,6 +1246,66 @@ def compute_gt_errors(self, obj_handle: str) -> None: "normalized_errors" ] = normalized_error + def grid_search_vhacd_params(self, obj_template_handle: str): + """ + For a specified set of search parameters, try all combinations by: + 1. computing new proxy shape + 2. evaluating new proxy shape across various metrics + """ + + vhacd_test_params = habitat_sim.VHACDParameters() + + self.compute_vhacd_col_shape(obj_template_handle, vhacd_test_params) + + self.compute_proxy_metrics(obj_template_handle) + + if self.compute_receptacle_useability_metrics: + self.compute_receptacle_access_metrics(obj_handle=obj_template_handle) + + def compute_vhacd_col_shape( + self, obj_template_handle: str, vhacd_params: habitat_sim.VHACDParameters + ) -> None: + """ + Compute a new VHACD convex decomposition for the object and set it as the active collision proxy. + """ + + new_template_handle = None + + otm = self.mm.object_template_manager + matching_obj_handles = otm.get_file_template_handles(obj_template_handle) + assert ( + len(matching_obj_handles) == 1 + ), f"None or many matching handles to substring `{obj_template_handle}`: {matching_obj_handles}" + obj_template = otm.get_template_by_handle(matching_obj_handles[0]) + render_asset = obj_template.render_asset_handle + render_asset_path = os.path.abspath(render_asset) + print(f"render_asset_path = {render_asset_path}") + + cfg = self.get_cfg_with_mm() + with habitat_sim.Simulator(cfg) as sim: + new_template_handle = sim.apply_convex_hull_decomposition( + render_asset_path, vhacd_params, save_chd_to_obj=True + ) + + print(f"new_template_handle = {new_template_handle}") + + # set the collision asset + matching_vhacd_handles = otm.get_file_template_handles(new_template_handle) + assert ( + len(matching_vhacd_handles) == 1 + ), f"None or many matching VHACD handles to substring `{new_template_handle}`: {matching_vhacd_handles}" + + vhacd_template = otm.get_template_by_handle(matching_vhacd_handles[0]) + + print(f"vhacd_template.csv_info = {vhacd_template.csv_info}") + print(f"obj_template.csv_info = {obj_template.csv_info}") + + obj_template.collision_asset_handle = vhacd_template.collision_asset_handle + print( + f"vhacd_template.collision_asset_handle = {vhacd_template.collision_asset_handle}" + ) + otm.register_template(obj_template) + def cache_global_results(self) -> None: """ Cache the current global cumulative results. @@ -1417,6 +1477,7 @@ def compute_and_save_results_for_objects( self.compute_receptacle_access_metrics(obj_h, use_gt=True) print(" PR Recetpacle Metrics:") self.compute_receptacle_access_metrics(obj_h, use_gt=False) + self.grid_search_vhacd_params(obj_h) self.compute_gt_errors(obj_h) self.cache_global_results() self.clean_obj_gt(obj_h) From 5dabf57320f1924d12d040d75223975288ca2ca6 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 14 Mar 2023 15:40:37 -0700 Subject: [PATCH 016/153] bugfix shape id tracking. VHACD collision shape optimization for select subset of objects --- tools/collision_shape_automation.py | 125 +++++++++++++++++++++------- 1 file changed, 95 insertions(+), 30 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 0e99c4db0f..8c189e0613 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -745,6 +745,7 @@ def setup_obj_gt( assert obj_template is not None, f"Could not find object handle `{obj_handle}`" self.gt_data[obj_handle] = {} + self.gt_data[obj_handle]["next_proxy_id"] = 0 # correct now for any COM automation obj_template.compute_COM_from_shape = False @@ -967,11 +968,8 @@ def compute_proxy_metrics(self, obj_handle: str) -> None: ), f"`{obj_handle}` does not have any entry in gt_data: {self.gt_data.keys()}. Call to `setup_obj_gt(obj_handle)` required." # when evaluating multiple proxy shapes, need unique ids: - pr_id = "pr0" - id_counter = 0 - while pr_id in self.gt_data[obj_handle]["raycasts"]: - pr_id = "pr" + str(id_counter) - id_counter += 1 + pr_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']}" + self.gt_data[obj_handle]["next_proxy_id"] += 1 # start with empty scene cfg = self.get_cfg_with_mm() @@ -1063,15 +1061,7 @@ def compute_receptacle_access_metrics( assert obj.is_alive, "Object was not added correctly." # when evaluating multiple proxy shapes, need unique ids: - pr_id = None - next_pr_id = "pr0" - id_counter = 0 - while next_pr_id in self.gt_data[obj_handle]["raycasts"]: - pr_id = next_pr_id - next_pr_id = "pr" + str(id_counter) - id_counter += 1 - assert pr_id is not None - shape_id = pr_id + shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" # gather hemisphere rays scaled to object's size # NOTE: because the receptacle points can be located anywhere in the bounding box, raycast radius must be bb diagonal length @@ -1253,14 +1243,78 @@ def grid_search_vhacd_params(self, obj_template_handle: str): 2. evaluating new proxy shape across various metrics """ - vhacd_test_params = habitat_sim.VHACDParameters() + # vhacd_test_params = habitat_sim.VHACDParameters() + + # vhacd_test_params.max_num_vertices_per_ch = 64 + # vhacd_test_params.max_convex_hulls = 1024 + # vhacd_test_params.plane_downsampling = 4 #1-16 + # vhacd_test_params.convex_hull_downsampling = 4 #1-16 + + # vhacd_test_params.alpha = 0.05 #bias towards symmetry [0-1] + # vhacd_test_params.beta = 0.05 #bias toward revolution axes [0-1] + + # vhacd_test_params.mode = 0 #0=voxel, 1=tetrahedral + # vhacd_test_params.pca = 0 #1=use PCA normalization + + param_ranges = { + # "pca": (0,1), #pca seems worse, no speed improvement + # "mode": (0,1), #tetrahedral mode seems worse + "alpha": (0.05, 0.1), + "beta": (0.05, 0.1), + "plane_downsampling": [2], + "convex_hull_downsampling": [2], + "max_num_vertices_per_ch": (16, 32), + # "max_convex_hulls": (8,16,32,64), + "max_convex_hulls": (16, 32, 64), + "resolution": [200000], + } + + permutations = [[]] + + # permute variations + for attr, values in param_ranges.items(): + new_permutations = [] + for v in values: + for permutation in permutations: + extended_permutation = [(attr, v)] + for setting in permutation: + extended_permutation.append(setting) + new_permutations.append(extended_permutation) + permutations = new_permutations + print(f"Parameter permutations = {len(permutations)}") + for setting in permutations: + print(f" {setting}") + + vhacd_start_time = time.time() + vhacd_iteration_times = {} + # evaluate VHACD settings + for setting in permutations: + vhacd_params = habitat_sim.VHACDParameters() + setting_string = "" + for attr, val in setting: + setattr(vhacd_params, attr, val) + setting_string += f" '{attr}'={val}" + vhacd_iteration_time = time.time() + self.compute_vhacd_col_shape(obj_template_handle, vhacd_params) + + self.compute_proxy_metrics(obj_template_handle) + proxy_id = f"pr{self.gt_data[obj_template_handle]['next_proxy_id']-1}" + if "vhacd_settings" not in self.gt_data[obj_template_handle]: + self.gt_data[obj_template_handle]["vhacd_settings"] = {} + self.gt_data[obj_template_handle]["vhacd_settings"][ + proxy_id + ] = setting_string - self.compute_vhacd_col_shape(obj_template_handle, vhacd_test_params) - - self.compute_proxy_metrics(obj_template_handle) + if self.compute_receptacle_useability_metrics: + self.compute_receptacle_access_metrics(obj_handle=obj_template_handle) + vhacd_iteration_times[proxy_id] = time.time() - vhacd_iteration_time - if self.compute_receptacle_useability_metrics: - self.compute_receptacle_access_metrics(obj_handle=obj_template_handle) + print(f"Total VHACD time = {time.time()-vhacd_start_time}") + print(" Iteration times = ") + for proxy_id, settings in self.gt_data[obj_template_handle][ + "vhacd_settings" + ].items(): + print(f" {proxy_id} - {settings} - {vhacd_iteration_times[proxy_id]}") def compute_vhacd_col_shape( self, obj_template_handle: str, vhacd_params: habitat_sim.VHACDParameters @@ -1279,7 +1333,6 @@ def compute_vhacd_col_shape( obj_template = otm.get_template_by_handle(matching_obj_handles[0]) render_asset = obj_template.render_asset_handle render_asset_path = os.path.abspath(render_asset) - print(f"render_asset_path = {render_asset_path}") cfg = self.get_cfg_with_mm() with habitat_sim.Simulator(cfg) as sim: @@ -1287,8 +1340,6 @@ def compute_vhacd_col_shape( render_asset_path, vhacd_params, save_chd_to_obj=True ) - print(f"new_template_handle = {new_template_handle}") - # set the collision asset matching_vhacd_handles = otm.get_file_template_handles(new_template_handle) assert ( @@ -1297,13 +1348,8 @@ def compute_vhacd_col_shape( vhacd_template = otm.get_template_by_handle(matching_vhacd_handles[0]) - print(f"vhacd_template.csv_info = {vhacd_template.csv_info}") - print(f"obj_template.csv_info = {obj_template.csv_info}") - obj_template.collision_asset_handle = vhacd_template.collision_asset_handle - print( - f"vhacd_template.collision_asset_handle = {vhacd_template.collision_asset_handle}" - ) + otm.register_template(obj_template) def cache_global_results(self) -> None: @@ -1550,7 +1596,26 @@ def main(): if object_has_receptacles(all_handles[i], otm) ] print(f"Number of objects with receptacles = {len(all_handles)}") - all_handles = all_handles[:100] + + # NOTE: select objects for testing VHACD pipeline + target_object_handles = [ + "01be253cbfd14b947e9dbe09d0b1959e97d72122", # desk + "01b65339d622bb9f89eb8fdd753a76cffc7eb8d6", # shelves, + "00ea83bf1b2544df87f6d52d02382c0bb75598c6", # bookcase + "00e388a751b3654216f2109ee073dc44f1241eee", # counter + "01d9fff2f701af7d5d40a7a5adad5bf40d4c49c8", # round table + "03c328fccef4975310314838e42b6dff06709b06", # shelves + "0110c7ff0e787bf98c9da923554ddea1484e4a3d", # wood table + "00366b86401aa16b702c21de49fd59b75ab9c57b", # ratan sofa + ] + all_handles = [ + h for h in all_handles if any([t in h for t in target_object_handles]) + ] + + # indexed subset of the objects + # all_handles = all_handles[1:2] + + # print(all_handles) cpo.compute_and_save_results_for_objects(all_handles) # testing objects From c4567102e057b46fef6ebe79179a1654e745482f Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 15 Mar 2023 14:43:39 -0700 Subject: [PATCH 017/153] receptacle stability analysis --- tools/collision_shape_automation.py | 168 +++++++++++++++++++++++++++- 1 file changed, 165 insertions(+), 3 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 8c189e0613..5dcba4c44b 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -15,6 +15,7 @@ import habitat.sims.habitat_simulator.debug_visualizer as hab_debug_vis import magnum as mn import numpy as np +from habitat.sims.habitat_simulator.sim_utilities import snap_down import habitat_sim from habitat_sim.utils.settings import default_sim_settings, make_cfg @@ -1025,7 +1026,7 @@ def compute_receptacle_access_metrics( Compute a heuristic for the accessibility of all Receptacles for an object. Uses raycasting from previously sampled receptacle locations to approximate how open a particular receptacle is. :param use_gt: Compute the metric for the ground truth shape instead of the currently active collision proxy (default) - :param acces_ratio_threshold: The ratio of accessible:blocked rays necessary for a recetpacle point to be considered accessible + :param acces_ratio_threshold: The ratio of accessible:blocked rays necessary for a Receptacle point to be considered accessible """ # algorithm: # For each receptacle, r: @@ -1206,6 +1207,164 @@ def compute_receptacle_access_metrics( ) # obj_rec_data[receptacle_name]["results"][shape_id]["sample_point_ray_results"] + def compute_receptacle_stability( + self, + obj_handle: str, + use_gt: bool = False, + cyl_radius: float = 0.04, + cyl_height: float = 0.15, + accepted_height_error: float = 0.02, + ): + """ + Try to place a dynamic cylinder on the receptacle points. Record snap error and physical stability. + + :param obj_handle: The object to evaluate. + :param use_gt: Compute the metric for the ground truth shape instead of the currently active collision proxy (default) + :param cyl_radius: Radius of the test cylinder object (default similar to food can) + :param cyl_height: Height of the test cylinder object (default similar to food can) + :param accepted_height_error: The acceptacle distance from sample point to snapped point considered successful (meters) + """ + + constructed_cyl_obj_handle = f"cylinder_test_obj_r{cyl_radius}_h{cyl_height}" + otm = self.mm.object_template_manager + if not otm.get_library_has_handle(constructed_cyl_obj_handle): + # ensure that a correctly sized asset mesh is available + atm = self.mm.asset_template_manager + half_length = (cyl_height / 2.0) / cyl_radius + custom_prim_name = f"cylinderSolid_rings_1_segments_12_halfLen_{half_length}_useTexCoords_false_useTangents_false_capEnds_true" + + if not atm.get_library_has_handle(custom_prim_name): + # build the primitive template + cylinder_prim_handle = atm.get_template_handles("cylinderSolid")[0] + cyl_template = atm.get_template_by_handle(cylinder_prim_handle) + # default radius==1, so we modify the half-length + cyl_template.half_length = half_length + atm.register_template(cyl_template) + + assert atm.get_library_has_handle( + custom_prim_name + ), "primitive asset creation bug." + + if not otm.get_library_has_handle(constructed_cyl_obj_handle): + default_cyl_template_handle = otm.get_synth_template_handles( + "cylinderSolid" + )[0] + default_cyl_template = otm.get_template_by_handle( + default_cyl_template_handle + ) + default_cyl_template.render_asset_handle = custom_prim_name + default_cyl_template.collision_asset_handle = custom_prim_name + # our prim asset has unit radius, so scale the object by desired radius + default_cyl_template.scale = mn.Vector3(cyl_radius) + otm.register_template(default_cyl_template, constructed_cyl_obj_handle) + assert otm.get_library_has_handle(constructed_cyl_obj_handle) + + assert ( + len(self.gt_data[obj_handle]["receptacles"].keys()) > 0 + ), "Object must have receptacle sampling metadata defined. See `setup_obj_gt`" + + # start with empty scene or stage as scene: + scene_name = "NONE" + if use_gt: + scene_name = self.gt_data[obj_handle]["stage_template_name"] + cfg = self.get_cfg_with_mm(scene=scene_name) + with habitat_sim.Simulator(cfg) as sim: + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + # load the object + rom = sim.get_rigid_object_manager() + obj = None + support_obj_ids = [-1] + shape_id = "gt" + if not use_gt: + obj = rom.add_object_by_template_handle(obj_handle) + support_obj_ids = [obj.object_id] + assert obj.is_alive, "Object was not added correctly." + # when evaluating multiple proxy shapes, need unique ids: + shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + + # add the test object + cyl_test_obj = rom.add_object_by_template_handle(constructed_cyl_obj_handle) + cyl_test_obj_com_height = cyl_test_obj.root_scene_node.cumulative_bb.max[1] + assert cyl_test_obj.is_alive, "Test object was not added correctly." + + # evaluation the sample points for each receptacle + rec_data = self.gt_data[obj_handle]["receptacles"] + for rec_name in rec_data.keys(): + sample_points = rec_data[rec_name]["sample_points"] + + failed_snap = 0 + failed_by_distance = 0 + failed_unstable = 0 + for sample_point in sample_points: + cyl_test_obj.translation = sample_point + cyl_test_obj.rotation = mn.Quaternion.identity_init() + # snap check + success = snap_down( + sim, cyl_test_obj, support_obj_ids=support_obj_ids, vdb=dvb + ) + if success: + expected_height_error = abs( + (cyl_test_obj.translation - sample_point).length() + - cyl_test_obj_com_height + ) + if expected_height_error > accepted_height_error: + failed_by_distance += 1 + continue + + # physical stability analysis + snap_position = cyl_test_obj.translation + identity_q = mn.Quaternion.identity_init() + displacement_limit = 0.04 # meters + rotation_limit = mn.Rad(0.1) # radians + max_sim_time = 3.0 + dt = 0.5 + start_time = sim.get_world_time() + object_is_stable = True + while sim.get_world_time() - start_time < max_sim_time: + sim.step_world(dt) + linear_displacement = ( + cyl_test_obj.translation - snap_position + ).length() + # NOTE: negative quaternion represents the same rotation, but gets a different angle error so check both + angular_displacement = min( + mn.math.angle(cyl_test_obj.rotation, identity_q), + mn.math.angle(-1 * cyl_test_obj.rotation, identity_q), + ) + if ( + angular_displacement > rotation_limit + or linear_displacement > displacement_limit + ): + object_is_stable = False + break + if not cyl_test_obj.awake: + # the object has settled, no need to continue simulating + break + # NOTE: we assume that if the object has not moved past the threshold in 'max_sim_time', then it must be stabel enough + if not object_is_stable: + failed_unstable += 1 + else: + failed_snap += 1 + + successful_points = ( + len(sample_points) + - failed_snap + - failed_by_distance + - failed_unstable + ) + success_ratio = successful_points / len(sample_points) + print( + f"{shape_id}: receptacle '{rec_name}' success_ratio = {success_ratio}" + ) + print( + f" failed_snap = {failed_snap}|failed_by_distance = {failed_by_distance}|failed_unstable={failed_unstable}|total={len(sample_points)}" + ) + # TODO: visualize this error + # TODO: record results for later processing + def compute_gt_errors(self, obj_handle: str) -> None: """ Compute and cache all ground truth error metrics. @@ -1492,6 +1651,7 @@ def save_results_to_csv(self, filename: str) -> None: else: row_data.append("") writer.writerow(row_data) + # TODO: add receptacle stability reporting def compute_and_save_results_for_objects( self, obj_handle_substrings: List[str], output_filename: str = "cpo_out" @@ -1519,9 +1679,11 @@ def compute_and_save_results_for_objects( self.compute_proxy_metrics(obj_h) # receptacle metrics: if self.compute_receptacle_useability_metrics: - print(" GT Recetpacle Metrics:") + self.compute_receptacle_stability(obj_h, use_gt=True) + self.compute_receptacle_stability(obj_h) + print(" GT Receptacle Metrics:") self.compute_receptacle_access_metrics(obj_h, use_gt=True) - print(" PR Recetpacle Metrics:") + print(" PR Receptacle Metrics:") self.compute_receptacle_access_metrics(obj_h, use_gt=False) self.grid_search_vhacd_params(obj_h) self.compute_gt_errors(obj_h) From c4ae110c2a1683635947753bc2fc36a69cc2e98a Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 24 Mar 2023 09:50:27 -0700 Subject: [PATCH 018/153] initial physics metric tests --- tools/collision_shape_automation.py | 305 +++++++++++++++++++++++++++- 1 file changed, 303 insertions(+), 2 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 5dcba4c44b..aa1f723791 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -904,6 +904,9 @@ def setup_obj_gt( "gt": {"results": gt_raycast_results} } + # setup data cache entries for physics test info + self.gt_data[obj_handle]["physics_test_info"] = {} + def clean_obj_gt(self, obj_handle: str) -> None: """ Cleans the global object cache to better manage process memory. @@ -1283,6 +1286,8 @@ def compute_receptacle_stability( obj = rom.add_object_by_template_handle(obj_handle) support_obj_ids = [obj.object_id] assert obj.is_alive, "Object was not added correctly." + # need to make the object STATIC so it doesn't move + obj.motion_type = habitat_sim.physics.MotionType.STATIC # when evaluating multiple proxy shapes, need unique ids: shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" @@ -1365,6 +1370,293 @@ def compute_receptacle_stability( # TODO: visualize this error # TODO: record results for later processing + def run_physics_settle_test(self, obj_handle): + """ + Drops the object on a plane and waits for it to sleep. + Provides a heuristic measure of dynamic stability. If the object jitters, bounces, or oscillates it won't sleep. + """ + + cfg = self.get_cfg_with_mm() + with habitat_sim.Simulator(cfg) as sim: + rom = sim.get_rigid_object_manager() + obj = rom.add_object_by_template_handle(obj_handle) + assert obj.is_alive, "Object was not added correctly." + + # when evaluating multiple proxy shapes, need unique ids: + shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + + # add a plane + otm = sim.get_object_template_manager() + cube_plane_handle = "cubePlaneSolid" + if not otm.get_library_has_handle(cube_plane_handle): + cube_prim_handle = otm.get_template_handles("cubeSolid")[0] + cube_template = otm.get_template_by_handle(cube_prim_handle) + cube_template.scale = mn.Vector3(20, 0.05, 20) + otm.register_template(cube_template, cube_plane_handle) + assert otm.get_library_has_handle(cube_plane_handle) + plane_obj = rom.add_object_by_template_handle(cube_plane_handle) + assert plane_obj.is_alive, "Plane object was not added correctly." + plane_obj.motion_type = habitat_sim.physics.MotionType.STATIC + + # use DebugVisualizer to get 6-axis view of the object + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + dvb.peek_rigid_object( + obj, peek_all_axis=True, additional_savefile_prefix="plane_snap_" + ) + + # snap the object to the plane + obj.translation = mn.Vector3(0, 0.6, 0) + success = snap_down(sim, obj, support_obj_ids=[plane_obj.object_id]) + + if not success: + print("Failed to snap object to plane...") + return + + # simulate for settling + max_sim_time = 5.0 + dt = 0.25 + real_start_time = time.time() + object_is_stable = False + start_time = sim.get_world_time() + while sim.get_world_time() - start_time < max_sim_time: + sim.step_world(dt) + dvb.peek_rigid_object( + obj, + peek_all_axis=True, + additional_savefile_prefix=f"plane_snap_{sim.get_world_time() - start_time}_", + ) + + if not obj.awake: + object_is_stable = True + # the object has settled, no need to continue simulating + break + real_test_time = time.time() - real_start_time + sim_settle_time = sim.get_world_time() - start_time + print(f"Physics Settle Time Report: '{obj_handle}'") + if object_is_stable: + print(f" Settled in {sim_settle_time} sim seconds.") + else: + print(f" Failed to settle in {max_sim_time} sim seconds.") + print(f" Test completed in {real_test_time} seconds.") + + if "settle_report" not in self.gt_data[obj_handle]["physics_test_info"]: + self.gt_data[obj_handle]["physics_test_info"]["settle_report"] = {} + + assert ( + shape_id + not in self.gt_data[obj_handle]["physics_test_info"]["settle_report"] + ), f"Duplicate settle report entry for shape_id {shape_id}." + + self.gt_data[obj_handle]["physics_test_info"]["settle_report"][shape_id] = { + "success": object_is_stable, + "realtime": real_test_time, + "max_time": max_sim_time, + "settle_time": sim_settle_time, + } + + def compute_grid_collision_times(self, obj_handle, subdivisions=0, use_gt=False): + """ + Runs a collision test over a subdivided grid of box shapes within the object's AABB. + Measures discrete collision check efficiency. + + "param subdivisions": number of recursive subdivisions to create the grid. E.g. 0 is the bb, 1 is 8 box of 1/2 bb size, etc... + """ + + scene_name = "NONE" + if use_gt: + scene_name = self.gt_data[obj_handle]["stage_template_name"] + cfg = self.get_cfg_with_mm(scene=scene_name) + with habitat_sim.Simulator(cfg) as sim: + rom = sim.get_rigid_object_manager() + shape_id = "gt" + shape_bb = None + if not use_gt: + obj = rom.add_object_by_template_handle(obj_handle) + assert obj.is_alive, "Object was not added correctly." + # need to make the object STATIC so it doesn't move + obj.motion_type = habitat_sim.physics.MotionType.STATIC + # when evaluating multiple proxy shapes, need unique ids: + shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + shape_bb = obj.root_scene_node.cumulative_bb + else: + shape_bb = sim.get_active_scene_graph().get_root_node().cumulative_bb + + # add the collision box + otm = sim.get_object_template_manager() + cube_prim_handle = otm.get_template_handles("cubeSolid")[0] + cube_template = otm.get_template_by_handle(cube_prim_handle) + num_segments = 2**subdivisions + subdivision_scale = 1.0 / (num_segments) + cube_template.scale = shape_bb.size() * subdivision_scale + # TODO: test this scale + otm.register_template(cube_template, "cubeTestSolid") + + test_obj = rom.add_object_by_template_handle("cubeTestSolid") + assert test_obj.is_alive, "Test box object was not added correctly." + + cell_scale = cube_template.scale + # run the grid test + test_start_time = time.time() + max_col_time = 0 + for x in range(num_segments): + for y in range(num_segments): + for z in range(num_segments): + box_center = ( + shape_bb.min + + mn.Vector3.x_axis(cell_scale[0]) * x + + mn.Vector3.y_axis(cell_scale[1]) * y + + mn.Vector3.z_axis(cell_scale[2]) * z + + cell_scale / 2.0 + ) + test_obj.translation = box_center + col_start = time.time() + test_obj.contact_test() + col_time = time.time() - col_start + max_col_time = max(max_col_time, col_time) + total_test_time = time.time() - test_start_time + avg_test_time = total_test_time / (num_segments**3) + + print( + f"Physics grid collision test report: {obj_handle}. {subdivisions} subdivisions." + ) + print( + f" Test took {total_test_time} seconds for {num_segments**3} collision tests." + ) + + # TODO: test this + + if ( + "collision_grid_report" + not in self.gt_data[obj_handle]["physics_test_info"] + ): + self.gt_data[obj_handle]["physics_test_info"][ + "collision_grid_report" + ] = {} + + if ( + shape_id + not in self.gt_data[obj_handle]["physics_test_info"][ + "collision_grid_report" + ] + ): + self.gt_data[obj_handle]["physics_test_info"]["collision_grid_report"][ + shape_id + ] = {} + + self.gt_data[obj_handle]["physics_test_info"]["collision_grid_report"][ + shape_id + ][subdivisions] = { + "total_col_time": total_test_time, + "avg_col_time": avg_test_time, + "max_col_time": max_col_time, + } + + def run_physics_sphere_shake_test(self, obj_handle): + """ + Places the DYNAMIC object in a sphere with other primitives and varies gravity to mix the objects. + Per-frame physics compute time serves as a metric for dynamic simulation efficiency. + """ + + # prepare a sphere stage + sphere_radius = self.gt_data[obj_handle]["scene_bb"].size().length() * 1.5 + sphere_stage_handle = "sphereTestStage" + stm = self.mm.stage_template_manager + sphere_template = stm.create_new_template(sphere_stage_handle) + sphere_template.render_asset_handle = "data/test_assets/objects/sphere.glb" + sphere_template.scale = mn.Vector3(sphere_radius * 2.0) # glb is radius 0.5 + stm.register_template(sphere_template, sphere_stage_handle) + + # prepare the test sphere object + otm = self.mm.object_template_manager + sphere_test_handle = "sphereTestCollisionObject" + sphere_prim_handle = otm.get_template_handles("sphereSolid")[0] + sphere_template = otm.get_template_by_handle(sphere_prim_handle) + test_sphere_radius = sphere_radius / 100.0 + sphere_template.scale = mn.Vector3(test_sphere_radius) + otm.register_template(sphere_template, sphere_test_handle) + assert otm.get_library_has_handle(sphere_test_handle) + + shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + + cfg = self.get_cfg_with_mm(scene=sphere_stage_handle) + with habitat_sim.Simulator(cfg) as sim: + rom = sim.get_rigid_object_manager() + obj = rom.add_object_by_template_handle(obj_handle) + assert obj.is_alive, "Object was not added correctly." + + # fill the remaining space with small spheres + num_spheres = 0 + while num_spheres < 100: + sphere_obj = rom.add_object_by_template_handle(sphere_test_handle) + assert sphere_obj.is_alive, "Object was not added correctly." + num_tries = 0 + while num_tries < 50: + num_tries += 1 + # sample point + new_point = mn.Vector3(np.random.random(3) * 2.0 - np.ones(1)) + while new_point.length() >= 0.99: + new_point = mn.Vector3(np.random.random(3) * 2.0 - np.ones(1)) + sphere_obj.translation = new_point + if not sphere_obj.contact_test(): + num_spheres += 1 + break + if num_tries == 50: + # we hit our max, so end the search + rom.remove_object_by_handle(sphere_obj.handle) + break + + # run the simulation for timing + gravity = sim.get_gravity() + grav_rotation_rate = 0.5 # revolutions per second + max_sim_time = 10.0 + dt = 0.25 + real_start_time = time.time() + start_time = sim.get_world_time() + while sim.get_world_time() - start_time < max_sim_time: + sim.step_world(dt) + # change gravity + cur_time = sim.get_world_time() - start_time + grav_revolutions = grav_rotation_rate * cur_time + # rotate the gravity vector around the Z axis + g_quat = mn.Quaternion.rotation( + mn.Rad(grav_revolutions * mn.math.pi * 2), mn.Vector3(0, 0, 1) + ) + sim.set_gravity(g_quat.transform_vector(gravity)) + + real_test_time = time.time() - real_start_time + + print(f"Physics 'sphere shake' report: {obj_handle}") + print( + f" {num_spheres} spheres took {real_test_time} seconds for {max_sim_time} sim seconds." + ) + + if ( + "sphere_shake_report" + not in self.gt_data[obj_handle]["physics_test_info"] + ): + self.gt_data[obj_handle]["physics_test_info"][ + "sphere_shake_report" + ] = {} + + assert ( + shape_id + not in self.gt_data[obj_handle]["physics_test_info"][ + "sphere_shake_report" + ] + ), f"Duplicate sphere shake report entry for shape_id {shape_id}." + + self.gt_data[obj_handle]["physics_test_info"]["sphere_shake_report"][ + shape_id + ] = { + "realtime": real_test_time, + "sim_time": max_sim_time, + "num_spheres": num_spheres, + } + def compute_gt_errors(self, obj_handle: str) -> None: """ Compute and cache all ground truth error metrics. @@ -1675,8 +1967,17 @@ def compute_and_save_results_for_objects( print(f"Computing metric for `{obj_h}`, {obix}|{len(obj_handles)}") print("-------------------------------") self.setup_obj_gt(obj_h) - self.compute_baseline_metrics(obj_h) - self.compute_proxy_metrics(obj_h) + # self.compute_baseline_metrics(obj_h) + # self.compute_proxy_metrics(obj_h) + + # physics tests + # self.run_physics_settle_test(obj_h) + # self.run_physics_sphere_shake_test(obj_h) + # self.compute_grid_collision_times(obj_h, subdivisions=0) + # self.compute_grid_collision_times(obj_h, subdivisions=1) + # self.compute_grid_collision_times(obj_h, subdivisions=2) + # exit() + # receptacle metrics: if self.compute_receptacle_useability_metrics: self.compute_receptacle_stability(obj_h, use_gt=True) From 585269a20b4f924f8b0ce8091cc5751580abc0ca Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 3 Apr 2023 14:48:15 -0700 Subject: [PATCH 019/153] add receptacle debug_draw in viewer. Limit processed objects to those found in a single scene. Re-organize receptacle metrics caching. --- examples/viewer.py | 22 ++++ tools/collision_shape_automation.py | 161 ++++++++++++++++++++-------- 2 files changed, 141 insertions(+), 42 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 8e2f4b2231..49b252df55 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -14,6 +14,7 @@ flags = sys.getdlopenflags() sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) +import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle import magnum as mn import numpy as np from magnum import shaders, text @@ -183,6 +184,9 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.mouse_cast_results = None self.debug_draw_raycasts = True + self.debug_draw_receptacles = True + self.object_receptacles = [] + # toggle a single simulation step at the next opportunity if not # simulating continuously. self.simulate_single_step = False @@ -324,6 +328,24 @@ def debug_draw(self): color=mn.Color4.magenta(), ) + if self.debug_draw_receptacles and self.collision_proxy_obj is not None: + # parse any receptacles defined for the object + if len(self.object_receptacles) == 0: + source_template_file = ( + self.collision_proxy_obj.creation_attributes.file_directory + ) + user_attr = self.collision_proxy_obj.user_attributes + self.object_receptacles = ( + hab_receptacle.parse_receptacles_from_user_config( + user_attr, + parent_object_handle=self.collision_proxy_obj.handle, + parent_template_directory=source_template_file, + ) + ) + # draw any receptacles for the object + for receptacle in self.object_receptacles: + receptacle.debug_draw(self.sim, color=mn.Color4.green()) + def draw_event( self, simulation_call: Optional[Callable] = None, diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index aa1f723791..d501474681 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -103,9 +103,32 @@ def get_scaled_hemisphere_vectors(scale: float): + """ + Scales the icosphere_points for use with raycasting applications. + """ return [v * scale for v in icosphere_points_subdiv_3] +def print_dict_structure(input_dict: Dict[Any, Any], whitespace: str = "") -> None: + """ + Quick structure investigation for dictionary. + Prints dict key->type recursively with incremental whitespace formatting. + """ + if whitespace == "": + print("-----------------------------------") + print("Print Dict Structure Results:") + for key in input_dict.keys(): + if isinstance(input_dict[key], Dict): + print(whitespace + f"{key}:-") + print_dict_structure( + input_dict=input_dict[key], whitespace=whitespace + " " + ) + else: + print(whitespace + f"{key}: {type(input_dict[key])}") + if whitespace == "": + print("-----------------------------------") + + # ======================================================================= # Range3D surface sampling utils @@ -1083,12 +1106,12 @@ def compute_receptacle_access_metrics( # collect hemisphere raycast samples for all receptacle sample points for receptacle_name in obj_rec_data.keys(): - if "results" not in obj_rec_data[receptacle_name]: - obj_rec_data[receptacle_name]["results"] = {} + if "access_results" not in obj_rec_data[receptacle_name]: + obj_rec_data[receptacle_name]["access_results"] = {} assert ( - shape_id not in obj_rec_data[receptacle_name]["results"] + shape_id not in obj_rec_data[receptacle_name]["access_results"] ), f" overwriting results for {shape_id}" - obj_rec_data[receptacle_name]["results"][shape_id] = {} + obj_rec_data[receptacle_name]["access_results"][shape_id] = {} sample_point_ray_results: List[ List[habitat_sim.physics.RaycastResults] ] = [] @@ -1126,13 +1149,13 @@ def compute_receptacle_access_metrics( receptacle_access_score /= len(sample_points) receptacle_access_rate /= len(sample_points) - obj_rec_data[receptacle_name]["results"][shape_id][ + obj_rec_data[receptacle_name]["access_results"][shape_id][ "sample_point_ray_results" ] = sample_point_ray_results - obj_rec_data[receptacle_name]["results"][shape_id][ + obj_rec_data[receptacle_name]["access_results"][shape_id][ "receptacle_access_score" ] = receptacle_access_score - obj_rec_data[receptacle_name]["results"][shape_id][ + obj_rec_data[receptacle_name]["access_results"][shape_id][ "receptacle_access_rate" ] = receptacle_access_rate print(f" receptacle_name = {receptacle_name}") @@ -1143,7 +1166,7 @@ def compute_receptacle_access_metrics( # generate receptacle access debug images # 1a Show missed rays vs 1b hit rays debug_lines = [] - for ray_results in obj_rec_data[receptacle_name]["results"][ + for ray_results in obj_rec_data[receptacle_name]["access_results"][ shape_id ]["sample_point_ray_results"]: for hit_record in ray_results: @@ -1368,7 +1391,20 @@ def compute_receptacle_stability( f" failed_snap = {failed_snap}|failed_by_distance = {failed_by_distance}|failed_unstable={failed_unstable}|total={len(sample_points)}" ) # TODO: visualize this error - # TODO: record results for later processing + + # write results to cache + if "stability_results" not in rec_data[rec_name]: + rec_data[rec_name]["stability_results"] = {} + assert ( + shape_id not in rec_data[rec_name]["stability_results"] + ), f" overwriting results for {shape_id}" + rec_data[rec_name]["stability_results"][shape_id] = { + "success_ratio": success_ratio, + "failed_snap": failed_snap, + "failed_by_distance": failed_by_distance, + "failed_unstable": failed_unstable, + "total": len(sample_points), + } def run_physics_settle_test(self, obj_handle): """ @@ -1828,11 +1864,13 @@ def cache_global_results(self) -> None: # cache the receptacle access metrics for CSV save for rec_key in self.gt_data[obj_handle]["receptacles"].keys(): self.results[obj_handle]["receptacle_info"][rec_key] = {} + assert ( - "results" in self.gt_data[obj_handle]["receptacles"][rec_key] + "access_results" + in self.gt_data[obj_handle]["receptacles"][rec_key] ), "Must run 'compute_receptacle_access_metrics' before caching global receptacle data." rec_results = self.gt_data[obj_handle]["receptacles"][rec_key][ - "results" + "access_results" ] # access rate and score @@ -1967,15 +2005,15 @@ def compute_and_save_results_for_objects( print(f"Computing metric for `{obj_h}`, {obix}|{len(obj_handles)}") print("-------------------------------") self.setup_obj_gt(obj_h) - # self.compute_baseline_metrics(obj_h) - # self.compute_proxy_metrics(obj_h) + self.compute_baseline_metrics(obj_h) + self.compute_proxy_metrics(obj_h) # physics tests - # self.run_physics_settle_test(obj_h) - # self.run_physics_sphere_shake_test(obj_h) - # self.compute_grid_collision_times(obj_h, subdivisions=0) - # self.compute_grid_collision_times(obj_h, subdivisions=1) - # self.compute_grid_collision_times(obj_h, subdivisions=2) + self.run_physics_settle_test(obj_h) + self.run_physics_sphere_shake_test(obj_h) + self.compute_grid_collision_times(obj_h, subdivisions=0) + self.compute_grid_collision_times(obj_h, subdivisions=1) + self.compute_grid_collision_times(obj_h, subdivisions=2) # exit() # receptacle metrics: @@ -1986,13 +2024,16 @@ def compute_and_save_results_for_objects( self.compute_receptacle_access_metrics(obj_h, use_gt=True) print(" PR Receptacle Metrics:") self.compute_receptacle_access_metrics(obj_h, use_gt=False) - self.grid_search_vhacd_params(obj_h) + # self.grid_search_vhacd_params(obj_h) self.compute_gt_errors(obj_h) - self.cache_global_results() + self.cache_global_results2() + print_dict_structure(self.gt_data) + print_dict_structure(self.results) + # exit() self.clean_obj_gt(obj_h) # then save results to file - self.save_results_to_csv(output_filename) + self.save_results_to_csv2(output_filename) def object_has_receptacles( @@ -2018,6 +2059,29 @@ def object_has_receptacles( ) +def get_objects_in_scene( + dataset_path: str, scene_handle: str, mm: habitat_sim.metadata.MetadataMediator +) -> List[str]: + """ + Load a scene and return a list of object template handles for all instantiated objects. + """ + sim_settings = default_sim_settings.copy() + sim_settings["scene_dataset_config_file"] = dataset_path + sim_settings["scene"] = scene_handle + + cfg = make_cfg(sim_settings) + cfg.metadata_mediator = mm + + with habitat_sim.Simulator(cfg) as sim: + scene_object_template_handles = [] + rom = sim.get_rigid_object_manager() + live_objects = rom.get_objects_by_handle_substring() + for _obj_handle, obj in live_objects.items(): + if obj.creation_attributes.handle not in scene_object_template_handles: + scene_object_template_handles.append(obj.creation_attributes.handle) + return scene_object_template_handles + + def main(): parser = argparse.ArgumentParser( description="Automate collision shape creation and validation." @@ -2048,10 +2112,23 @@ def main(): # use the CollisionProxyOptimizer to compute metrics for multiple objects cpo = CollisionProxyOptimizer(sim_settings, output_directory=args.output_dir) cpo.generate_debug_images = True + otm = cpo.mm.object_template_manager + + # ---------------------------------------------------- + # get object handles from a specific scene + scene_of_interest = "107734119_175999932" + objects_in_scene = get_objects_in_scene( + dataset_path=args.dataset, scene_handle=scene_of_interest, mm=cpo.mm + ) + all_handles = objects_in_scene + # ---------------------------------------------------- + # ---------------------------------------------------- # get all object handles - otm = cpo.mm.object_template_manager - all_handles = otm.get_file_template_handles() + # all_handles = otm.get_file_template_handles() + # ---------------------------------------------------- + + # ---------------------------------------------------- # get a subset with receptacles defined all_handles = [ all_handles[i] @@ -2059,33 +2136,33 @@ def main(): if object_has_receptacles(all_handles[i], otm) ] print(f"Number of objects with receptacles = {len(all_handles)}") + # ---------------------------------------------------- + # ---------------------------------------------------- # NOTE: select objects for testing VHACD pipeline - target_object_handles = [ - "01be253cbfd14b947e9dbe09d0b1959e97d72122", # desk - "01b65339d622bb9f89eb8fdd753a76cffc7eb8d6", # shelves, - "00ea83bf1b2544df87f6d52d02382c0bb75598c6", # bookcase - "00e388a751b3654216f2109ee073dc44f1241eee", # counter - "01d9fff2f701af7d5d40a7a5adad5bf40d4c49c8", # round table - "03c328fccef4975310314838e42b6dff06709b06", # shelves - "0110c7ff0e787bf98c9da923554ddea1484e4a3d", # wood table - "00366b86401aa16b702c21de49fd59b75ab9c57b", # ratan sofa - ] - all_handles = [ - h for h in all_handles if any([t in h for t in target_object_handles]) - ] - + # target_object_handles = [ + # "01be253cbfd14b947e9dbe09d0b1959e97d72122", # desk + # "01b65339d622bb9f89eb8fdd753a76cffc7eb8d6", # shelves, + # "00ea83bf1b2544df87f6d52d02382c0bb75598c6", # bookcase + # "00e388a751b3654216f2109ee073dc44f1241eee", # counter + # "01d9fff2f701af7d5d40a7a5adad5bf40d4c49c8", # round table + # "03c328fccef4975310314838e42b6dff06709b06", # shelves + # "0110c7ff0e787bf98c9da923554ddea1484e4a3d", # wood table + # "00366b86401aa16b702c21de49fd59b75ab9c57b", # ratan sofa + # ] + # all_handles = [ + # h for h in all_handles if any([t in h for t in target_object_handles]) + # ] + # ---------------------------------------------------- + + # ---------------------------------------------------- # indexed subset of the objects # all_handles = all_handles[1:2] + # ---------------------------------------------------- # print(all_handles) cpo.compute_and_save_results_for_objects(all_handles) - # testing objects - # obj_handle1 = "0a5e809804911e71de6a4ef89f2c8fef5b9291b3" - # obj_handle2 = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" - # cpo.compute_and_save_results_for_objects([obj_handle1, obj_handle2]) - if __name__ == "__main__": main() From d86da23cc94986895bcc29ad08dd2612299f127e Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 3 Apr 2023 19:25:25 -0700 Subject: [PATCH 020/153] refactor to align data cache protocal with design --- tools/collision_shape_automation.py | 495 +++++++++++++++------------- 1 file changed, 275 insertions(+), 220 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index d501474681..ad23ba6467 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -735,6 +735,25 @@ def __init__(self, sim_settings: Dict[str, Any], output_directory="") -> None: # cache global results to be written to csv. self.results: Dict[str, Dict[str, Any]] = {} + def get_proxy_index(self, obj_handle: str) -> int: + """ + Get the current proxy index for an object. + """ + return self.gt_data[obj_handle]["proxy_index"] + + def increment_proxy_index(self, obj_handle: str) -> int: + """ + Increment the current proxy index. + Only do this after all processing for the current proxy is complete. + """ + self.gt_data[obj_handle]["proxy_index"] += 1 + + def get_proxy_shape_id(self, obj_handle: str) -> str: + """ + Get a string representation of the current proxy shape. + """ + return f"pr{self.get_proxy_index(obj_handle)}" + def get_cfg_with_mm( self, scene: str = "NONE" ) -> habitat_sim.simulator.Configuration: @@ -768,8 +787,25 @@ def setup_obj_gt( obj_template = otm.get_template_by_handle(obj_handle) assert obj_template is not None, f"Could not find object handle `{obj_handle}`" - self.gt_data[obj_handle] = {} - self.gt_data[obj_handle]["next_proxy_id"] = 0 + # create a stage template with the object's render mesh as a "ground truth" for metrics + stm = self.mm.stage_template_manager + stage_template_name = obj_handle + "_as_stage" + new_stage_template = stm.create_new_template(handle=stage_template_name) + new_stage_template.render_asset_handle = obj_template.render_asset_handle + stm.register_template( + template=new_stage_template, specified_handle=stage_template_name + ) + + # initialize the object's runtime data cache + self.gt_data[obj_handle] = { + "proxy_index": 0, # used to recover and increment `shape_id` during optimization and evaluation + "stage_template_name": stage_template_name, + "receptacles": {}, # sub-cache for receptacle metric data and results + "raycasts": {}, # subcache for shape raycasting metric data + "shape_test_results": { + "gt": {} + }, # subcache for shape and physics metric results + } # correct now for any COM automation obj_template.compute_COM_from_shape = False @@ -797,7 +833,6 @@ def setup_obj_gt( ) # sample test points on the receptacles - self.gt_data[obj_handle]["receptacles"] = {} for receptacle in obj_receptacles: if type(receptacle) == hab_receptacle.TriangleMeshReceptacle: rec_test_points = [] @@ -819,7 +854,8 @@ def setup_obj_gt( # ) # ) self.gt_data[obj_handle]["receptacles"][receptacle.name] = { - "sample_points": rec_test_points + "sample_points": rec_test_points, + "shape_id_results": {}, } if self.generate_debug_images: debug_lines = [] @@ -875,14 +911,6 @@ def setup_obj_gt( ) # load a simulator instance with this object as the stage - stm = self.mm.stage_template_manager - stage_template_name = obj_handle + "_as_stage" - self.gt_data[obj_handle]["stage_template_name"] = stage_template_name - new_stage_template = stm.create_new_template(handle=stage_template_name) - new_stage_template.render_asset_handle = obj_template.render_asset_handle - stm.register_template( - template=new_stage_template, specified_handle=stage_template_name - ) cfg = self.get_cfg_with_mm(scene=stage_template_name) with habitat_sim.Simulator(cfg) as sim: # get test points from bounding box info: @@ -923,12 +951,7 @@ def setup_obj_gt( # compute and cache "ground truth" raycast on object as stage gt_raycast_results = run_pairwise_raycasts(test_points, sim) - self.gt_data[obj_handle]["raycasts"] = { - "gt": {"results": gt_raycast_results} - } - - # setup data cache entries for physics test info - self.gt_data[obj_handle]["physics_test_info"] = {} + self.gt_data[obj_handle]["raycasts"]["gt"] = gt_raycast_results def clean_obj_gt(self, obj_handle: str) -> None: """ @@ -956,9 +979,7 @@ def compute_baseline_metrics(self, obj_handle: str) -> None: empty_raycast_results = run_pairwise_raycasts( self.gt_data[obj_handle]["test_points"], sim ) - self.gt_data[obj_handle]["raycasts"]["empty"] = { - "results": empty_raycast_results - } + self.gt_data[obj_handle]["raycasts"]["empty"] = empty_raycast_results cfg = self.get_cfg_with_mm() with habitat_sim.Simulator(cfg) as sim: @@ -980,7 +1001,7 @@ def compute_baseline_metrics(self, obj_handle: str) -> None: bb_raycast_results = run_pairwise_raycasts( self.gt_data[obj_handle]["test_points"], sim ) - self.gt_data[obj_handle]["raycasts"]["bb"] = {"results": bb_raycast_results} + self.gt_data[obj_handle]["raycasts"]["bb"] = bb_raycast_results # un-modify the template obj_template.bounding_box_collisions = False @@ -995,8 +1016,7 @@ def compute_proxy_metrics(self, obj_handle: str) -> None: ), f"`{obj_handle}` does not have any entry in gt_data: {self.gt_data.keys()}. Call to `setup_obj_gt(obj_handle)` required." # when evaluating multiple proxy shapes, need unique ids: - pr_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']}" - self.gt_data[obj_handle]["next_proxy_id"] += 1 + pr_id = self.get_proxy_shape_id(obj_handle) # start with empty scene cfg = self.get_cfg_with_mm() @@ -1037,9 +1057,7 @@ def compute_proxy_metrics(self, obj_handle: str) -> None: pr_raycast_results = run_pairwise_raycasts( self.gt_data[obj_handle]["test_points"], sim ) - self.gt_data[obj_handle]["raycasts"][pr_id] = { - "results": pr_raycast_results - } + self.gt_data[obj_handle]["raycasts"][pr_id] = pr_raycast_results # undo template modification obj_template.render_asset_handle = render_asset @@ -1088,7 +1106,7 @@ def compute_receptacle_access_metrics( assert obj.is_alive, "Object was not added correctly." # when evaluating multiple proxy shapes, need unique ids: - shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + shape_id = self.get_proxy_shape_id(obj_handle) # gather hemisphere rays scaled to object's size # NOTE: because the receptacle points can be located anywhere in the bounding box, raycast radius must be bb diagonal length @@ -1106,12 +1124,6 @@ def compute_receptacle_access_metrics( # collect hemisphere raycast samples for all receptacle sample points for receptacle_name in obj_rec_data.keys(): - if "access_results" not in obj_rec_data[receptacle_name]: - obj_rec_data[receptacle_name]["access_results"] = {} - assert ( - shape_id not in obj_rec_data[receptacle_name]["access_results"] - ), f" overwriting results for {shape_id}" - obj_rec_data[receptacle_name]["access_results"][shape_id] = {} sample_point_ray_results: List[ List[habitat_sim.physics.RaycastResults] ] = [] @@ -1149,15 +1161,23 @@ def compute_receptacle_access_metrics( receptacle_access_score /= len(sample_points) receptacle_access_rate /= len(sample_points) - obj_rec_data[receptacle_name]["access_results"][shape_id][ - "sample_point_ray_results" - ] = sample_point_ray_results - obj_rec_data[receptacle_name]["access_results"][shape_id][ - "receptacle_access_score" - ] = receptacle_access_score - obj_rec_data[receptacle_name]["access_results"][shape_id][ - "receptacle_access_rate" - ] = receptacle_access_rate + if shape_id not in obj_rec_data[receptacle_name]["shape_id_results"]: + obj_rec_data[receptacle_name]["shape_id_results"][shape_id] = {} + assert ( + "access_results" + not in obj_rec_data[receptacle_name]["shape_id_results"][shape_id] + ), f"Overwriting existing 'access_results' data for '{receptacle_name}'|'{shape_id}'." + obj_rec_data[receptacle_name]["shape_id_results"][shape_id][ + "access_results" + ] = { + "sample_point_ray_results": sample_point_ray_results, + "receptacle_access_score": receptacle_access_score, + "receptacle_access_rate": receptacle_access_rate, + } + access_results = obj_rec_data[receptacle_name]["shape_id_results"][ + shape_id + ]["access_results"] + print(f" receptacle_name = {receptacle_name}") print(f" receptacle_access_score = {receptacle_access_score}") print(f" receptacle_access_rate = {receptacle_access_rate}") @@ -1166,9 +1186,7 @@ def compute_receptacle_access_metrics( # generate receptacle access debug images # 1a Show missed rays vs 1b hit rays debug_lines = [] - for ray_results in obj_rec_data[receptacle_name]["access_results"][ - shape_id - ]["sample_point_ray_results"]: + for ray_results in access_results["sample_point_ray_results"]: for hit_record in ray_results: if not hit_record.has_hits(): debug_lines.append( @@ -1312,7 +1330,7 @@ def compute_receptacle_stability( # need to make the object STATIC so it doesn't move obj.motion_type = habitat_sim.physics.MotionType.STATIC # when evaluating multiple proxy shapes, need unique ids: - shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + shape_id = self.get_proxy_shape_id(obj_handle) # add the test object cyl_test_obj = rom.add_object_by_template_handle(constructed_cyl_obj_handle) @@ -1393,12 +1411,15 @@ def compute_receptacle_stability( # TODO: visualize this error # write results to cache - if "stability_results" not in rec_data[rec_name]: - rec_data[rec_name]["stability_results"] = {} + if shape_id not in rec_data[rec_name]["shape_id_results"]: + rec_data[rec_name]["shape_id_results"][shape_id] = {} assert ( - shape_id not in rec_data[rec_name]["stability_results"] - ), f" overwriting results for {shape_id}" - rec_data[rec_name]["stability_results"][shape_id] = { + "stability_results" + not in rec_data[rec_name]["shape_id_results"][shape_id] + ), f"Overwriting existing 'stability_results' data for '{rec_name}'|'{shape_id}'." + rec_data[rec_name]["shape_id_results"][shape_id][ + "stability_results" + ] = { "success_ratio": success_ratio, "failed_snap": failed_snap, "failed_by_distance": failed_by_distance, @@ -1406,6 +1427,17 @@ def compute_receptacle_stability( "total": len(sample_points), } + def setup_shape_test_results_cache(self, obj_handle: str, shape_id: str) -> None: + """ + Ensure the 'shape_test_results' sub-cache is initialized for a 'shape_id'. + """ + if shape_id not in self.gt_data[obj_handle]["shape_test_results"]: + self.gt_data[obj_handle]["shape_test_results"][shape_id] = { + "settle_report": {}, + "sphere_shake_report": {}, + "collision_grid_report": {}, + } + def run_physics_settle_test(self, obj_handle): """ Drops the object on a plane and waits for it to sleep. @@ -1419,7 +1451,8 @@ def run_physics_settle_test(self, obj_handle): assert obj.is_alive, "Object was not added correctly." # when evaluating multiple proxy shapes, need unique ids: - shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + shape_id = self.get_proxy_shape_id(obj_handle) + self.setup_shape_test_results_cache(obj_handle, shape_id) # add a plane otm = sim.get_object_template_manager() @@ -1450,6 +1483,14 @@ def run_physics_settle_test(self, obj_handle): if not success: print("Failed to snap object to plane...") + self.gt_data[obj_handle]["shape_test_results"][shape_id][ + "settle_report" + ] = { + "success": False, + "realtime": "NA", + "max_time": "NA", + "settle_time": "NA", + } return # simulate for settling @@ -1479,15 +1520,9 @@ def run_physics_settle_test(self, obj_handle): print(f" Failed to settle in {max_sim_time} sim seconds.") print(f" Test completed in {real_test_time} seconds.") - if "settle_report" not in self.gt_data[obj_handle]["physics_test_info"]: - self.gt_data[obj_handle]["physics_test_info"]["settle_report"] = {} - - assert ( - shape_id - not in self.gt_data[obj_handle]["physics_test_info"]["settle_report"] - ), f"Duplicate settle report entry for shape_id {shape_id}." - - self.gt_data[obj_handle]["physics_test_info"]["settle_report"][shape_id] = { + self.gt_data[obj_handle]["shape_test_results"][shape_id][ + "settle_report" + ] = { "success": object_is_stable, "realtime": real_test_time, "max_time": max_sim_time, @@ -1516,11 +1551,13 @@ def compute_grid_collision_times(self, obj_handle, subdivisions=0, use_gt=False) # need to make the object STATIC so it doesn't move obj.motion_type = habitat_sim.physics.MotionType.STATIC # when evaluating multiple proxy shapes, need unique ids: - shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + shape_id = self.get_proxy_shape_id(obj_handle) shape_bb = obj.root_scene_node.cumulative_bb else: shape_bb = sim.get_active_scene_graph().get_root_node().cumulative_bb + self.setup_shape_test_results_cache(obj_handle, shape_id) + # add the collision box otm = sim.get_object_template_manager() cube_prim_handle = otm.get_template_handles("cubeSolid")[0] @@ -1565,26 +1602,8 @@ def compute_grid_collision_times(self, obj_handle, subdivisions=0, use_gt=False) # TODO: test this - if ( + self.gt_data[obj_handle]["shape_test_results"][shape_id][ "collision_grid_report" - not in self.gt_data[obj_handle]["physics_test_info"] - ): - self.gt_data[obj_handle]["physics_test_info"][ - "collision_grid_report" - ] = {} - - if ( - shape_id - not in self.gt_data[obj_handle]["physics_test_info"][ - "collision_grid_report" - ] - ): - self.gt_data[obj_handle]["physics_test_info"]["collision_grid_report"][ - shape_id - ] = {} - - self.gt_data[obj_handle]["physics_test_info"]["collision_grid_report"][ - shape_id ][subdivisions] = { "total_col_time": total_test_time, "avg_col_time": avg_test_time, @@ -1616,7 +1635,8 @@ def run_physics_sphere_shake_test(self, obj_handle): otm.register_template(sphere_template, sphere_test_handle) assert otm.get_library_has_handle(sphere_test_handle) - shape_id = f"pr{self.gt_data[obj_handle]['next_proxy_id']-1}" + shape_id = self.get_proxy_shape_id(obj_handle) + self.setup_shape_test_results_cache(obj_handle, shape_id) cfg = self.get_cfg_with_mm(scene=sphere_stage_handle) with habitat_sim.Simulator(cfg) as sim: @@ -1670,23 +1690,8 @@ def run_physics_sphere_shake_test(self, obj_handle): f" {num_spheres} spheres took {real_test_time} seconds for {max_sim_time} sim seconds." ) - if ( + self.gt_data[obj_handle]["shape_test_results"][shape_id][ "sphere_shake_report" - not in self.gt_data[obj_handle]["physics_test_info"] - ): - self.gt_data[obj_handle]["physics_test_info"][ - "sphere_shake_report" - ] = {} - - assert ( - shape_id - not in self.gt_data[obj_handle]["physics_test_info"][ - "sphere_shake_report" - ] - ), f"Duplicate sphere shake report entry for shape_id {shape_id}." - - self.gt_data[obj_handle]["physics_test_info"]["sphere_shake_report"][ - shape_id ] = { "realtime": real_test_time, "sim_time": max_sim_time, @@ -1710,17 +1715,19 @@ def compute_gt_errors(self, obj_handle: str) -> None: "gt" in self.gt_data[obj_handle]["raycasts"] ), "Must have a ground truth to compare against. Should be generated in `setup_obj_gt(obj_handle)`." - for key in self.gt_data[obj_handle]["raycasts"].keys(): + for shape_id in self.gt_data[obj_handle]["raycasts"].keys(): + self.setup_shape_test_results_cache(obj_handle, shape_id) if ( - key != "gt" - and "normalized_errors" not in self.gt_data[obj_handle]["raycasts"][key] + shape_id != "gt" + and "normalized_raycast_error" + not in self.gt_data[obj_handle]["shape_test_results"][shape_id] ): normalized_error = get_raycast_results_cumulative_error_metric( - self.gt_data[obj_handle]["raycasts"]["gt"]["results"], - self.gt_data[obj_handle]["raycasts"][key]["results"], + self.gt_data[obj_handle]["raycasts"]["gt"], + self.gt_data[obj_handle]["raycasts"][shape_id], ) - self.gt_data[obj_handle]["raycasts"][key][ - "normalized_errors" + self.gt_data[obj_handle]["shape_test_results"][shape_id][ + "normalized_raycast_error" ] = normalized_error def grid_search_vhacd_params(self, obj_template_handle: str): @@ -1782,26 +1789,36 @@ def grid_search_vhacd_params(self, obj_template_handle: str): setattr(vhacd_params, attr, val) setting_string += f" '{attr}'={val}" vhacd_iteration_time = time.time() + # create new shape and increment shape id self.compute_vhacd_col_shape(obj_template_handle, vhacd_params) + self.increment_proxy_index(obj_template_handle) - self.compute_proxy_metrics(obj_template_handle) - proxy_id = f"pr{self.gt_data[obj_template_handle]['next_proxy_id']-1}" + # cache vhacd settings + shape_id = self.get_proxy_shape_id(obj_template_handle) if "vhacd_settings" not in self.gt_data[obj_template_handle]: self.gt_data[obj_template_handle]["vhacd_settings"] = {} self.gt_data[obj_template_handle]["vhacd_settings"][ - proxy_id + shape_id ] = setting_string + # compute shape level metrics: + self.compute_proxy_metrics(obj_template_handle) + self.compute_grid_collision_times(obj_template_handle, subdivisions=1) + self.run_physics_settle_test(obj_template_handle) + self.run_physics_sphere_shake_test(obj_template_handle) + + # compute receptacle metrics if self.compute_receptacle_useability_metrics: self.compute_receptacle_access_metrics(obj_handle=obj_template_handle) - vhacd_iteration_times[proxy_id] = time.time() - vhacd_iteration_time + self.compute_receptacle_stability(obj_handle=obj_template_handle) + vhacd_iteration_times[shape_id] = time.time() - vhacd_iteration_time print(f"Total VHACD time = {time.time()-vhacd_start_time}") print(" Iteration times = ") - for proxy_id, settings in self.gt_data[obj_template_handle][ + for shape_id, settings in self.gt_data[obj_template_handle][ "vhacd_settings" ].items(): - print(f" {proxy_id} - {settings} - {vhacd_iteration_times[proxy_id]}") + print(f" {shape_id} - {settings} - {vhacd_iteration_times[shape_id]}") def compute_vhacd_col_shape( self, obj_template_handle: str, vhacd_params: habitat_sim.VHACDParameters @@ -1846,48 +1863,81 @@ def cache_global_results(self) -> None: """ for obj_handle in self.gt_data.keys(): + # populate the high-level sub-cache definitions if obj_handle not in self.results: - self.results[obj_handle] = {} - for key in self.gt_data[obj_handle]["raycasts"].keys(): - if ( - key != "gt" - and "normalized_errors" in self.gt_data[obj_handle]["raycasts"][key] - ): - if "normalized_errors" not in self.results[obj_handle]: - self.results[obj_handle]["normalized_errors"] = {} - self.results[obj_handle]["normalized_errors"][key] = self.gt_data[ - obj_handle - ]["raycasts"][key]["normalized_errors"] - - if self.compute_receptacle_useability_metrics: - self.results[obj_handle]["receptacle_info"] = {} - # cache the receptacle access metrics for CSV save - for rec_key in self.gt_data[obj_handle]["receptacles"].keys(): - self.results[obj_handle]["receptacle_info"][rec_key] = {} - - assert ( - "access_results" - in self.gt_data[obj_handle]["receptacles"][rec_key] - ), "Must run 'compute_receptacle_access_metrics' before caching global receptacle data." - rec_results = self.gt_data[obj_handle]["receptacles"][rec_key][ - "access_results" + self.results[obj_handle] = { + "shape_metrics": {}, + "receptacle_metrics": {}, + } + # populate the per-shape metric sub-cache + for shape_id, shape_results in self.gt_data[obj_handle][ + "shape_test_results" + ].items(): + if shape_id == "gt": + continue + self.results[obj_handle]["shape_metrics"][shape_id] = {"col_grid": {}} + sm = self.results[obj_handle]["shape_metrics"][shape_id] + if "normalized_raycast_error" in shape_results: + sm["normalized_raycast_error"] = shape_results[ + "normalized_raycast_error" ] - - # access rate and score - for metric in ["receptacle_access_score", "receptacle_access_rate"]: - for shape_id in rec_results.keys(): - if ( - metric - not in self.results[obj_handle]["receptacle_info"][ - rec_key - ] - ): - self.results[obj_handle]["receptacle_info"][rec_key][ - metric - ] = {} - self.results[obj_handle]["receptacle_info"][rec_key][ - metric - ][shape_id] = rec_results[shape_id][metric] + if len(shape_results["settle_report"]) > 0: + sm["settle_success"] = shape_results["settle_report"]["success"] + sm["settle_time"] = shape_results["settle_report"]["settle_time"] + sm["settle_max_step_time"] = shape_results["settle_report"][ + "max_time" + ] + sm["settle_realtime"] = shape_results["settle_report"]["realtime"] + if len(shape_results["sphere_shake_report"]) > 0: + sm["shake_simtime"] = shape_results["sphere_shake_report"][ + "sim_time" + ] + sm["shake_realtime"] = shape_results["sphere_shake_report"][ + "realtime" + ] + sm["shake_num_spheres"] = shape_results["sphere_shake_report"][ + "num_spheres" + ] + if len(shape_results["collision_grid_report"]) > 0: + for subdiv, col_subdiv_results in shape_results[ + "collision_grid_report" + ].items(): + sm["col_grid"][subdiv] = { + "total_time": col_subdiv_results["total_col_time"], + "avg_time": col_subdiv_results["avg_col_time"], + "max_time": col_subdiv_results["max_col_time"], + } + # populate the receptacle metric sub-cache + for rec_name, rec_data in self.gt_data[obj_handle]["receptacles"].items(): + self.results[obj_handle]["receptacle_metrics"][rec_name] = {} + for shape_id, shape_data in rec_data["shape_id_results"].items(): + self.results[obj_handle]["receptacle_metrics"][rec_name][ + shape_id + ] = {} + rsm = self.results[obj_handle]["receptacle_metrics"][rec_name][ + shape_id + ] + if "stability_results" in shape_data: + rsm["stability_success_ratio"] = shape_data[ + "stability_results" + ]["success_ratio"] + rsm["failed_snap"] = shape_data["stability_results"][ + "failed_snap" + ] + rsm["failed_by_distance"] = shape_data["stability_results"][ + "failed_by_distance" + ] + rsm["failed_unstable"] = shape_data["stability_results"][ + "failed_unstable" + ] + rsm["total"] = shape_data["stability_results"]["total"] + if "access_results" in shape_data: + rsm["receptacle_access_score"] = shape_data["access_results"][ + "receptacle_access_score" + ] + rsm["receptacle_access_rate"] = shape_data["access_results"][ + "receptacle_access_rate" + ] def save_results_to_csv(self, filename: str) -> None: """ @@ -1900,41 +1950,67 @@ def save_results_to_csv(self, filename: str) -> None: filepath = os.path.join(self.output_directory, filename) - # save normalized error csv + # first collect all active metrics to log + active_subdivs = [] + active_shape_metrics = [] + for _obj_handle, obj_results in self.results.items(): + for _shape_id, shape_results in obj_results["shape_metrics"].items(): + for metric in shape_results.keys(): + if metric == "col_grid": + for subdiv in shape_results["col_grid"].keys(): + if subdiv not in active_subdivs: + active_subdivs.append(subdiv) + else: + if metric not in active_shape_metrics: + active_shape_metrics.append(metric) + active_subdivs = sorted(active_subdivs) + + # save shape metric csv with open(filepath + ".csv", "w") as f: writer = csv.writer(f, quoting=csv.QUOTE_ALL) - # first collect all column names: - existing_cols = [] - for obj_handle in self.results.keys(): - if "normalized_errors" in self.results[obj_handle]: - for key in self.results[obj_handle]["normalized_errors"]: - if key not in existing_cols: - existing_cols.append(key) - # put the baselines first - ordered_cols = ["object_handle"] - if "empty" in existing_cols: - ordered_cols.append("empty") - if "bb" in existing_cols: - ordered_cols.append("bb") - for key in existing_cols: - if key not in ordered_cols: - ordered_cols.append(key) + # first collect all column names (metrics): + existing_cols = ["object_handle|shape_id"] + existing_cols.extend(active_shape_metrics) + for subdiv in active_subdivs: + existing_cols.append(f"col_grid_{subdiv}_total_time") + existing_cols.append(f"col_grid_{subdiv}_avg_time") + existing_cols.append(f"col_grid_{subdiv}_max_time") # write column names row - writer.writerow(ordered_cols) + writer.writerow(existing_cols) # write results rows - for obj_handle in self.results.keys(): - row_data = [obj_handle] - if "normalized_errors" in self.results[obj_handle]: - for key in ordered_cols: - if key != "object_handle": - if key in self.results[obj_handle]["normalized_errors"]: - row_data.append( - self.results[obj_handle]["normalized_errors"][key] - ) - else: + for obj_handle, obj_results in self.results.items(): + for shape_id, shape_results in obj_results["shape_metrics"].items(): + row_data = [obj_handle + "|" + shape_id] + for metric_key in active_shape_metrics: + if metric_key in shape_results: + row_data.append(shape_results[metric_key]) + else: + row_data.append("") + for subdiv in active_subdivs: + if subdiv in shape_results["col_grid"]: + row_data.append( + shape_results["col_grid"][subdiv]["total_time"] + ) + row_data.append( + shape_results["col_grid"][subdiv]["avg_time"] + ) + row_data.append( + shape_results["col_grid"][subdiv]["max_time"] + ) + else: + for _ in range(3): row_data.append("") - writer.writerow(row_data) + writer.writerow(row_data) + + # collect active receptacle metrics + active_rec_metrics = [] + for _obj_handle, obj_results in self.results.items(): + for _rec_name, rec_results in obj_results["receptacle_metrics"].items(): + for _shape_id, shape_results in rec_results.items(): + for metric in shape_results.keys(): + if metric not in active_rec_metrics: + active_rec_metrics.append(metric) # export receptacle metrics to CSV if self.compute_receptacle_useability_metrics: @@ -1942,46 +2018,26 @@ def save_results_to_csv(self, filename: str) -> None: with open(rec_filepath + ".csv", "w") as f: writer = csv.writer(f, quoting=csv.QUOTE_ALL) # first collect all column names: - existing_cols = ["receptacle"] - shape_ids = [] - metrics = [] - for obj_handle in self.results.keys(): - if "receptacle_info" in self.results[obj_handle]: - for rec_key in self.results[obj_handle]["receptacle_info"]: - for metric in self.results[obj_handle]["receptacle_info"][ - rec_key - ].keys(): - if metric not in metrics: - metrics.append(metric) - for shape_id in self.results[obj_handle][ - "receptacle_info" - ][rec_key][metric].keys(): - if shape_id not in shape_ids: - shape_ids.append(shape_id) - if shape_id + "-" + metric not in existing_cols: - existing_cols.append(shape_id + "-" + metric) + existing_cols = ["obj_handle|receptacle|shape_id"] + existing_cols.extend(active_rec_metrics) + # write column names row writer.writerow(existing_cols) # write results rows - for obj_handle in self.results.keys(): - if "receptacle_info" in self.results[obj_handle]: - for rec_key in self.results[obj_handle]["receptacle_info"]: - rec_info = self.results[obj_handle]["receptacle_info"][ - rec_key - ] - row_data = [obj_handle + "_" + rec_key] - for metric in metrics: - for shape_id in shape_ids: - if ( - metric in rec_info - and shape_id in rec_info[metric] - ): - row_data.append(rec_info[metric][shape_id]) - else: - row_data.append("") + for obj_handle, obj_results in self.results.items(): + for rec_name, rec_results in obj_results[ + "receptacle_metrics" + ].items(): + for shape_id, shape_results in rec_results.items(): + row_data = [obj_handle + "|" + rec_name + "|" + shape_id] + for metric_key in active_rec_metrics: + if metric_key in shape_results: + row_data.append(shape_results[metric_key]) + else: + row_data.append("") + # write row data writer.writerow(row_data) - # TODO: add receptacle stability reporting def compute_and_save_results_for_objects( self, obj_handle_substrings: List[str], output_filename: str = "cpo_out" @@ -2014,7 +2070,6 @@ def compute_and_save_results_for_objects( self.compute_grid_collision_times(obj_h, subdivisions=0) self.compute_grid_collision_times(obj_h, subdivisions=1) self.compute_grid_collision_times(obj_h, subdivisions=2) - # exit() # receptacle metrics: if self.compute_receptacle_useability_metrics: @@ -2026,14 +2081,13 @@ def compute_and_save_results_for_objects( self.compute_receptacle_access_metrics(obj_h, use_gt=False) # self.grid_search_vhacd_params(obj_h) self.compute_gt_errors(obj_h) - self.cache_global_results2() print_dict_structure(self.gt_data) + self.cache_global_results() print_dict_structure(self.results) - # exit() self.clean_obj_gt(obj_h) # then save results to file - self.save_results_to_csv2(output_filename) + self.save_results_to_csv(output_filename) def object_has_receptacles( @@ -2136,6 +2190,7 @@ def main(): if object_has_receptacles(all_handles[i], otm) ] print(f"Number of objects with receptacles = {len(all_handles)}") + # ---------------------------------------------------- # ---------------------------------------------------- From 1ab696af02fe20297f42dabf35cf582681ff31fd Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 4 Apr 2023 18:22:54 -0700 Subject: [PATCH 021/153] minor settle test fix --- tools/collision_shape_automation.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index ad23ba6467..56d216a784 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1474,11 +1474,14 @@ def run_physics_settle_test(self, obj_handle): default_sensor_uuid="color_sensor", ) dvb.peek_rigid_object( - obj, peek_all_axis=True, additional_savefile_prefix="plane_snap_" + obj, + peek_all_axis=True, + additional_savefile_prefix=f"plane_snap_{shape_id}_", ) # snap the object to the plane - obj.translation = mn.Vector3(0, 0.6, 0) + obj_col_bb = obj.collision_shape_aabb + obj.translation = mn.Vector3(0, obj_col_bb.max[1] - obj_col_bb.min[1], 0) success = snap_down(sim, obj, support_obj_ids=[plane_obj.object_id]) if not success: @@ -1501,11 +1504,11 @@ def run_physics_settle_test(self, obj_handle): start_time = sim.get_world_time() while sim.get_world_time() - start_time < max_sim_time: sim.step_world(dt) - dvb.peek_rigid_object( - obj, - peek_all_axis=True, - additional_savefile_prefix=f"plane_snap_{sim.get_world_time() - start_time}_", - ) + # dvb.peek_rigid_object( + # obj, + # peek_all_axis=True, + # additional_savefile_prefix=f"plane_snap_{sim.get_world_time() - start_time}_", + # ) if not obj.awake: object_is_stable = True @@ -2079,7 +2082,7 @@ def compute_and_save_results_for_objects( self.compute_receptacle_access_metrics(obj_h, use_gt=True) print(" PR Receptacle Metrics:") self.compute_receptacle_access_metrics(obj_h, use_gt=False) - # self.grid_search_vhacd_params(obj_h) + self.grid_search_vhacd_params(obj_h) self.compute_gt_errors(obj_h) print_dict_structure(self.gt_data) self.cache_global_results() @@ -2189,6 +2192,7 @@ def main(): for i in range(len(all_handles)) if object_has_receptacles(all_handles[i], otm) ] + # all_handles = [all_handles[0]] print(f"Number of objects with receptacles = {len(all_handles)}") # ---------------------------------------------------- From 18d0750410bf8f12a6922ecb587050cdc83d4f55 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 4 Apr 2023 18:23:43 -0700 Subject: [PATCH 022/153] add viewer snapping util for receptacle debugging --- examples/viewer.py | 80 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/examples/viewer.py b/examples/viewer.py index 49b252df55..150d30ac31 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -17,6 +17,7 @@ import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle import magnum as mn import numpy as np +from habitat.sims.habitat_simulator.sim_utilities import snap_down from magnum import shaders, text from magnum.platform.glfw import Application @@ -314,6 +315,9 @@ def debug_draw(self): obj_temp_handle ) ) + self.collision_proxy_obj.motion_type = ( + habitat_sim.physics.MotionType.KINEMATIC + ) csa.debug_draw_raycast_results( self.sim, gt_raycast_results, pr_raycast_results, seed=self.sample_seed @@ -481,6 +485,8 @@ def reconfigure_sim(self) -> None: stage_template_name = "obj_as_stage_template" new_stage_template = stm.create_new_template(handle=stage_template_name) new_stage_template.render_asset_handle = obj_template.render_asset_handle + # margin must be 0 for snapping to work on overlapped gt/proxy + new_stage_template.margin = 0.0 stm.register_template( template=new_stage_template, specified_handle=stage_template_name ) @@ -834,6 +840,47 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: self.redraw() event.accepted = True + def construct_cylinder_object( + self, + cyl_radius: float = 0.04, + cyl_height: float = 0.15, + ): + constructed_cyl_obj_handle = f"cylinder_test_obj_r{cyl_radius}_h{cyl_height}" + + otm = self.sim.metadata_mediator.object_template_manager + if not otm.get_library_has_handle(constructed_cyl_obj_handle): + # ensure that a correctly sized asset mesh is available + atm = self.sim.metadata_mediator.asset_template_manager + half_length = (cyl_height / 2.0) / cyl_radius + custom_prim_name = f"cylinderSolid_rings_1_segments_12_halfLen_{half_length}_useTexCoords_false_useTangents_false_capEnds_true" + + if not atm.get_library_has_handle(custom_prim_name): + # build the primitive template + cylinder_prim_handle = atm.get_template_handles("cylinderSolid")[0] + cyl_template = atm.get_template_by_handle(cylinder_prim_handle) + # default radius==1, so we modify the half-length + cyl_template.half_length = half_length + atm.register_template(cyl_template) + + assert atm.get_library_has_handle( + custom_prim_name + ), "primitive asset creation bug." + + if not otm.get_library_has_handle(constructed_cyl_obj_handle): + default_cyl_template_handle = otm.get_synth_template_handles( + "cylinderSolid" + )[0] + default_cyl_template = otm.get_template_by_handle( + default_cyl_template_handle + ) + default_cyl_template.render_asset_handle = custom_prim_name + default_cyl_template.collision_asset_handle = custom_prim_name + # our prim asset has unit radius, so scale the object by desired radius + default_cyl_template.scale = mn.Vector3(cyl_radius) + otm.register_template(default_cyl_template, constructed_cyl_obj_handle) + assert otm.get_library_has_handle(constructed_cyl_obj_handle) + return constructed_cyl_obj_handle + def mouse_press_event(self, event: Application.MouseEvent) -> None: """ Handles `Application.MouseEvent`. When in GRAB mode, click on @@ -926,6 +973,38 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: # end if didn't hit the scene # end has raycast hit # end has physics enabled + elif ( + self.mouse_interaction == MouseMode.LOOK + and physics_enabled + and self.mouse_cast_results is not None + and self.mouse_cast_results.has_hits() + and event.button == button.RIGHT + ): + constructed_cyl_obj_handle = self.construct_cylinder_object() + # try to place an object + if ( + mn.math.dot( + self.mouse_cast_results.hits[0].normal.normalized(), + mn.Vector3(0, 1, 0), + ) + > 0.5 + ): + rom = self.sim.get_rigid_object_manager() + cyl_test_obj = rom.add_object_by_template_handle( + constructed_cyl_obj_handle + ) + assert cyl_test_obj is not None + cyl_test_obj.translation = self.mouse_cast_results.hits[ + 0 + ].point + mn.Vector3(0, 0.04, 0) + success = snap_down( + self.sim, + cyl_test_obj, + support_obj_ids=[-1, self.collision_proxy_obj.object_id], + ) + print(success) + if not success: + rom.remove_object_by_handle(cyl_test_obj.handle) self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() @@ -1349,6 +1428,7 @@ def next_frame() -> None: sim_settings["window_height"] = args.height sim_settings["default_agent_navmesh"] = False sim_settings["enable_hbao"] = args.hbao + sim_settings["clear_color"] = mn.Color4.magenta() obj_name = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" From 899b2bc9d37397adb4e38fa52544e74dbb87135f Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 7 Apr 2023 07:51:26 -0700 Subject: [PATCH 023/153] simpler constructed cylinder and receptacle stability test improvements --- examples/viewer.py | 46 ++++---------------- tools/collision_shape_automation.py | 66 +++++++++++++---------------- 2 files changed, 37 insertions(+), 75 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 150d30ac31..ba27adbe97 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -841,45 +841,15 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: event.accepted = True def construct_cylinder_object( - self, - cyl_radius: float = 0.04, - cyl_height: float = 0.15, + self, cyl_radius: float = 0.04, cyl_height: float = 0.15 ): - constructed_cyl_obj_handle = f"cylinder_test_obj_r{cyl_radius}_h{cyl_height}" - + constructed_cyl_temp_name = "scaled_cyl_template" otm = self.sim.metadata_mediator.object_template_manager - if not otm.get_library_has_handle(constructed_cyl_obj_handle): - # ensure that a correctly sized asset mesh is available - atm = self.sim.metadata_mediator.asset_template_manager - half_length = (cyl_height / 2.0) / cyl_radius - custom_prim_name = f"cylinderSolid_rings_1_segments_12_halfLen_{half_length}_useTexCoords_false_useTangents_false_capEnds_true" - - if not atm.get_library_has_handle(custom_prim_name): - # build the primitive template - cylinder_prim_handle = atm.get_template_handles("cylinderSolid")[0] - cyl_template = atm.get_template_by_handle(cylinder_prim_handle) - # default radius==1, so we modify the half-length - cyl_template.half_length = half_length - atm.register_template(cyl_template) - - assert atm.get_library_has_handle( - custom_prim_name - ), "primitive asset creation bug." - - if not otm.get_library_has_handle(constructed_cyl_obj_handle): - default_cyl_template_handle = otm.get_synth_template_handles( - "cylinderSolid" - )[0] - default_cyl_template = otm.get_template_by_handle( - default_cyl_template_handle - ) - default_cyl_template.render_asset_handle = custom_prim_name - default_cyl_template.collision_asset_handle = custom_prim_name - # our prim asset has unit radius, so scale the object by desired radius - default_cyl_template.scale = mn.Vector3(cyl_radius) - otm.register_template(default_cyl_template, constructed_cyl_obj_handle) - assert otm.get_library_has_handle(constructed_cyl_obj_handle) - return constructed_cyl_obj_handle + cyl_temp_handle = otm.get_synth_template_handles("cylinder")[0] + cyl_temp = otm.get_template_by_handle(cyl_temp_handle) + cyl_temp.scale = mn.Vector3(cyl_radius, cyl_height / 2.0, cyl_radius) + otm.register_template(cyl_temp, constructed_cyl_temp_name) + return constructed_cyl_temp_name def mouse_press_event(self, event: Application.MouseEvent) -> None: """ @@ -980,7 +950,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: and self.mouse_cast_results.has_hits() and event.button == button.RIGHT ): - constructed_cyl_obj_handle = self.construct_cylinder_object() + constructed_cyl_obj_handle = self.construct_cylinder_object2() # try to place an object if ( mn.math.dot( diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 56d216a784..9e47cbf341 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -726,7 +726,7 @@ def __init__(self, sim_settings: Dict[str, Any], output_directory="") -> None: # option to use Receptacle annotations to compute an additional accuracy metric self.compute_receptacle_useability_metrics = True # add a vertical epsilon offset to the receptacle points for analysis. This is added directly to the sampled points. - self.rec_point_vertical_offset = 0.02 + self.rec_point_vertical_offset = 0.041 # cache of test points, rays, distances, etc... for use by active processes # NOTE: entries created by `setup_obj_gt` and cleaned by `clean_obj_gt` for memory efficiency. @@ -1251,13 +1251,27 @@ def compute_receptacle_access_metrics( ) # obj_rec_data[receptacle_name]["results"][shape_id]["sample_point_ray_results"] + def construct_cylinder_object( + self, + mm: habitat_sim.metadata.MetadataMediator, + cyl_radius: float = 0.04, + cyl_height: float = 0.15, + ): + constructed_cyl_temp_name = "scaled_cyl_template" + otm = mm.object_template_manager + cyl_temp_handle = otm.get_synth_template_handles("cylinder")[0] + cyl_temp = otm.get_template_by_handle(cyl_temp_handle) + cyl_temp.scale = mn.Vector3(cyl_radius, cyl_height / 2.0, cyl_radius) + otm.register_template(cyl_temp, constructed_cyl_temp_name) + return constructed_cyl_temp_name + def compute_receptacle_stability( self, obj_handle: str, use_gt: bool = False, cyl_radius: float = 0.04, cyl_height: float = 0.15, - accepted_height_error: float = 0.02, + accepted_height_error: float = 0.05, ): """ Try to place a dynamic cylinder on the receptacle points. Record snap error and physical stability. @@ -1266,42 +1280,12 @@ def compute_receptacle_stability( :param use_gt: Compute the metric for the ground truth shape instead of the currently active collision proxy (default) :param cyl_radius: Radius of the test cylinder object (default similar to food can) :param cyl_height: Height of the test cylinder object (default similar to food can) - :param accepted_height_error: The acceptacle distance from sample point to snapped point considered successful (meters) + :param accepted_height_error: The acceptacle distance from receptacle to snapped point considered successful (meters) """ - constructed_cyl_obj_handle = f"cylinder_test_obj_r{cyl_radius}_h{cyl_height}" - otm = self.mm.object_template_manager - if not otm.get_library_has_handle(constructed_cyl_obj_handle): - # ensure that a correctly sized asset mesh is available - atm = self.mm.asset_template_manager - half_length = (cyl_height / 2.0) / cyl_radius - custom_prim_name = f"cylinderSolid_rings_1_segments_12_halfLen_{half_length}_useTexCoords_false_useTangents_false_capEnds_true" - - if not atm.get_library_has_handle(custom_prim_name): - # build the primitive template - cylinder_prim_handle = atm.get_template_handles("cylinderSolid")[0] - cyl_template = atm.get_template_by_handle(cylinder_prim_handle) - # default radius==1, so we modify the half-length - cyl_template.half_length = half_length - atm.register_template(cyl_template) - - assert atm.get_library_has_handle( - custom_prim_name - ), "primitive asset creation bug." - - if not otm.get_library_has_handle(constructed_cyl_obj_handle): - default_cyl_template_handle = otm.get_synth_template_handles( - "cylinderSolid" - )[0] - default_cyl_template = otm.get_template_by_handle( - default_cyl_template_handle - ) - default_cyl_template.render_asset_handle = custom_prim_name - default_cyl_template.collision_asset_handle = custom_prim_name - # our prim asset has unit radius, so scale the object by desired radius - default_cyl_template.scale = mn.Vector3(cyl_radius) - otm.register_template(default_cyl_template, constructed_cyl_obj_handle) - assert otm.get_library_has_handle(constructed_cyl_obj_handle) + constructed_cyl_obj_handle = self.construct_cylinder_object( + self.mm, cyl_radius, cyl_height + ) assert ( len(self.gt_data[obj_handle]["receptacles"].keys()) > 0 @@ -1337,6 +1321,11 @@ def compute_receptacle_stability( cyl_test_obj_com_height = cyl_test_obj.root_scene_node.cumulative_bb.max[1] assert cyl_test_obj.is_alive, "Test object was not added correctly." + # we sample above the receptacle to account for margin, but we compare distance to the actual receptacle height + receptacle_sample_height_correction = mn.Vector3( + 0, -self.rec_point_vertical_offset, 0 + ) + # evaluation the sample points for each receptacle rec_data = self.gt_data[obj_handle]["receptacles"] for rec_name in rec_data.keys(): @@ -1354,7 +1343,10 @@ def compute_receptacle_stability( ) if success: expected_height_error = abs( - (cyl_test_obj.translation - sample_point).length() + ( + cyl_test_obj.translation + - (sample_point + receptacle_sample_height_correction) + ).length() - cyl_test_obj_com_height ) if expected_height_error > accepted_height_error: From 77dd58be7c3e9544914b390f3f67166622f00dca Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 12 Apr 2023 11:18:38 -0700 Subject: [PATCH 024/153] multi-colored render for different receptacles --- examples/viewer.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index ba27adbe97..3b50cf61ff 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -24,7 +24,7 @@ import habitat_sim from habitat_sim import ReplayRenderer, ReplayRendererConfiguration, physics from habitat_sim.logging import LoggingContext, logger -from habitat_sim.utils.common import quat_from_angle_axis +from habitat_sim.utils.common import d3_40_colors_rgb, quat_from_angle_axis from habitat_sim.utils.settings import default_sim_settings, make_cfg # add tools directory so I can import things to try them in the viewer @@ -347,8 +347,10 @@ def debug_draw(self): ) ) # draw any receptacles for the object - for receptacle in self.object_receptacles: - receptacle.debug_draw(self.sim, color=mn.Color4.green()) + for rix, receptacle in enumerate(self.object_receptacles): + c = d3_40_colors_rgb[rix] + rec_color = mn.Vector3(c[0], c[1], c[2]) / 256.0 + receptacle.debug_draw(self.sim, color=mn.Color4.from_xyz(rec_color)) def draw_event( self, From f82137bfd8328c1373c4aa5737153a855a2bb3ca Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 13 Apr 2023 17:38:15 -0700 Subject: [PATCH 025/153] re-align API with habitat-lab mesh_data changes. Add object re-orientation feature (note: does not correctly re-orient receptacles yet) --- examples/viewer.py | 24 +++++++ tools/collision_shape_automation.py | 106 +++++++++++++++++++++++++--- 2 files changed, 121 insertions(+), 9 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 3b50cf61ff..90c50754e8 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -478,6 +478,22 @@ def reconfigure_sim(self) -> None: self.cfg.metadata_mediator.active_dataset = self.sim_settings[ "scene_dataset_config_file" ] + if args.reorient_object: + obj_handle = ( + self.cfg.metadata_mediator.object_template_manager.get_template_handles( + args.scene + )[0] + ) + fp_models_metadata_file = ( + "/home/alexclegg/Documents/dev/fphab/fpModels_metadata.csv" + ) + obj_orientations = csa.parse_object_orientations_from_metadata_csv( + fp_models_metadata_file + ) + csa.correct_object_orientations( + [obj_handle], obj_orientations, self.cfg.metadata_mediator + ) + otm = self.cfg.metadata_mediator.object_template_manager obj_template = otm.get_template_by_handle(obj_temp_handle) obj_template.compute_COM_from_shape = False @@ -487,6 +503,9 @@ def reconfigure_sim(self) -> None: stage_template_name = "obj_as_stage_template" new_stage_template = stm.create_new_template(handle=stage_template_name) new_stage_template.render_asset_handle = obj_template.render_asset_handle + new_stage_template.orient_up = obj_template.orient_up + new_stage_template.orient_front = obj_template.orient_front + # margin must be 0 for snapping to work on overlapped gt/proxy new_stage_template.margin = 0.0 stm.register_template( @@ -1337,6 +1356,11 @@ def next_frame() -> None: action="store_true", help="disable physics simulation (default: False)", ) + parser.add_argument( + "--reorient-object", + action="store_true", + help="reorient the object based on the values in the config file.", + ) parser.add_argument( "--use-default-lighting", action="store_true", diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 9e47cbf341..9eba45d5ae 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -3,6 +3,7 @@ # LICENSE file in the root directory of this source tree. import argparse +import csv import math import os import random @@ -616,6 +617,8 @@ def evaluate_collision_shape( stage_template_name = "obj_as_stage_template" new_stage_template = stm.create_new_template(handle=stage_template_name) new_stage_template.render_asset_handle = obj_template.render_asset_handle + new_stage_template.orient_up = obj_template.orient_up + new_stage_template.orient_front = obj_template.orient_front stm.register_template( template=new_stage_template, specified_handle=stage_template_name ) @@ -792,6 +795,8 @@ def setup_obj_gt( stage_template_name = obj_handle + "_as_stage" new_stage_template = stm.create_new_template(handle=stage_template_name) new_stage_template.render_asset_handle = obj_template.render_asset_handle + new_stage_template.orient_up = obj_template.orient_up + new_stage_template.orient_front = obj_template.orient_front stm.register_template( template=new_stage_template, specified_handle=stage_template_name ) @@ -859,11 +864,8 @@ def setup_obj_gt( } if self.generate_debug_images: debug_lines = [] - assert ( - len(receptacle.mesh_data[1]) % 3 == 0 - ), "must be triangles" for face in range( - int(len(receptacle.mesh_data[1]) / 3) + int(len(receptacle.mesh_data.indices) / 3) ): verts = receptacle.get_face_verts(f_ix=face) for edge in range(3): @@ -2056,15 +2058,15 @@ def compute_and_save_results_for_objects( print(f"Computing metric for `{obj_h}`, {obix}|{len(obj_handles)}") print("-------------------------------") self.setup_obj_gt(obj_h) - self.compute_baseline_metrics(obj_h) + # self.compute_baseline_metrics(obj_h) self.compute_proxy_metrics(obj_h) # physics tests self.run_physics_settle_test(obj_h) self.run_physics_sphere_shake_test(obj_h) - self.compute_grid_collision_times(obj_h, subdivisions=0) - self.compute_grid_collision_times(obj_h, subdivisions=1) - self.compute_grid_collision_times(obj_h, subdivisions=2) + # self.compute_grid_collision_times(obj_h, subdivisions=0) + # self.compute_grid_collision_times(obj_h, subdivisions=1) + # self.compute_grid_collision_times(obj_h, subdivisions=2) # receptacle metrics: if self.compute_receptacle_useability_metrics: @@ -2074,7 +2076,7 @@ def compute_and_save_results_for_objects( self.compute_receptacle_access_metrics(obj_h, use_gt=True) print(" PR Receptacle Metrics:") self.compute_receptacle_access_metrics(obj_h, use_gt=False) - self.grid_search_vhacd_params(obj_h) + # self.grid_search_vhacd_params(obj_h) self.compute_gt_errors(obj_h) print_dict_structure(self.gt_data) self.cache_global_results() @@ -2131,6 +2133,77 @@ def get_objects_in_scene( return scene_object_template_handles +def parse_object_orientations_from_metadata_csv( + metadata_csv: str, +) -> Dict[str, Tuple[mn.Vector3, mn.Vector3]]: + """ + Parse the 'up' and 'front' vectors of objects from a csv metadata file. + + :param metadata_csv: The absolute filepath of the metadata CSV. + + :return: A Dict mapping object ids to a Tuple of up, front vectors. + """ + + def str_to_vec(vec_str: str) -> mn.Vector3: + """ + Convert a list of 3 comma separated strings into a Vector3. + """ + elem_str = [float(x) for x in vec_str.split(",")] + assert len(elem_str) == 3, f"string '{vec_str}' must be a 3 vec." + return mn.Vector3(tuple(elem_str)) + + orientations = {} + + with open(metadata_csv, newline="") as csvfile: + reader = csv.reader(csvfile, delimiter=",") + id_row_ix = -1 + up_row_ix = -1 + front_row_ix = -1 + for rix, data_row in enumerate(reader): + if rix == 0: + id_row_ix = data_row.index("id") + up_row_ix = data_row.index("up") + front_row_ix = data_row.index("front") + else: + up = data_row[up_row_ix] + front = data_row[front_row_ix] + if len(up) == 0 or len(front) == 0: + # both must be set or neither + assert len(up) == 0 + assert len(front) == 0 + else: + orientations[data_row[id_row_ix]] = ( + str_to_vec(up), + str_to_vec(front), + ) + + return orientations + + +def correct_object_orientations( + obj_handles: List[str], + obj_orientations: Dict[str, Tuple[mn.Vector3, mn.Vector3]], + mm: habitat_sim.metadata.MetadataMediator, +) -> None: + """ + Correct the orientations for all object templates in 'obj_handles' as specified by 'obj_orientations'. + + :param obj_handles: A list of object template handles. + :param obj_orientations: A dict mapping object names (abridged, not handles) to Tuple of (up,front) orientation vectors. + """ + obj_handle_to_orientation = {} + for obj_name in obj_orientations.keys(): + for obj_handle in obj_handles: + if obj_name in obj_handle: + obj_handle_to_orientation[obj_handle] = obj_orientations[obj_name] + print(f"obj_handle_to_orientation = {obj_handle_to_orientation}") + for obj_handle, orientation in obj_handle_to_orientation.items(): + obj_template = mm.object_template_manager.get_template_by_handle(obj_handle) + obj_template.orient_up = orientation[0] + obj_template.orient_front = orientation[1] + mm.object_template_manager.register_template(obj_template) + + def main(): parser = argparse.ArgumentParser( description="Automate collision shape creation and validation." @@ -2187,6 +2260,21 @@ def main(): # all_handles = [all_handles[0]] print(f"Number of objects with receptacles = {len(all_handles)}") + # ---------------------------------------------------- + # load object orientation metadata + reorient_objects = False + if reorient_objects: + fp_models_metadata_file = ( + "/home/alexclegg/Documents/dev/fphab/fpModels_metadata.csv" + ) + obj_orientations = parse_object_orientations_from_metadata_csv( + fp_models_metadata_file + ) + correct_object_orientations(all_handles, obj_orientations, cpo.mm) + # ---------------------------------------------------- + + print(f"all_handles[0] = {all_handles[0]}") + # ---------------------------------------------------- # ---------------------------------------------------- From 4f1a5ef3efd1b413faf46900b68f8e5963a92142 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 13 Apr 2023 18:10:40 -0700 Subject: [PATCH 026/153] enable running per-scene evaluations --- tools/collision_shape_automation.py | 110 +++++++++++++--------------- 1 file changed, 50 insertions(+), 60 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 9eba45d5ae..155edfa6bf 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -731,6 +731,12 @@ def __init__(self, sim_settings: Dict[str, Any], output_directory="") -> None: # add a vertical epsilon offset to the receptacle points for analysis. This is added directly to the sampled points. self.rec_point_vertical_offset = 0.041 + self.init_caches() + + def init_caches(self): + """ + Re-initialize all internal data caches to prepare for re-use. + """ # cache of test points, rays, distances, etc... for use by active processes # NOTE: entries created by `setup_obj_gt` and cleaned by `clean_obj_gt` for memory efficiency. self.gt_data: Dict[str, Dict[str, Any]] = {} @@ -2062,8 +2068,8 @@ def compute_and_save_results_for_objects( self.compute_proxy_metrics(obj_h) # physics tests - self.run_physics_settle_test(obj_h) - self.run_physics_sphere_shake_test(obj_h) + # self.run_physics_settle_test(obj_h) + # self.run_physics_sphere_shake_test(obj_h) # self.compute_grid_collision_times(obj_h, subdivisions=0) # self.compute_grid_collision_times(obj_h, subdivisions=1) # self.compute_grid_collision_times(obj_h, subdivisions=2) @@ -2215,9 +2221,6 @@ def main(): default="collision_shape_automation/", help="output directory for saved csv and images. Default = `./collision_shape_automation/`.", ) - # parser.add_argument( - # "--object-handle", type=str, help="handle identifying the object to evaluate." - # ) args = parser.parse_args() sim_settings = default_sim_settings.copy() @@ -2236,71 +2239,58 @@ def main(): cpo.generate_debug_images = True otm = cpo.mm.object_template_manager - # ---------------------------------------------------- - # get object handles from a specific scene - scene_of_interest = "107734119_175999932" - objects_in_scene = get_objects_in_scene( - dataset_path=args.dataset, scene_handle=scene_of_interest, mm=cpo.mm - ) - all_handles = objects_in_scene - # ---------------------------------------------------- - # ---------------------------------------------------- # get all object handles # all_handles = otm.get_file_template_handles() + # cpo.compute_and_save_results_for_objects(all_handles) # ---------------------------------------------------- # ---------------------------------------------------- - # get a subset with receptacles defined - all_handles = [ - all_handles[i] - for i in range(len(all_handles)) - if object_has_receptacles(all_handles[i], otm) + # run the pipeline for a set of scenes with separate output files for each + scenes_of_interest = [] + # get all scenes from the mm + scenes_of_interest = [ + handle.split(".scene_instance.json")[0].split("/")[-1] + for handle in cpo.mm.get_scene_handles() ] - # all_handles = [all_handles[0]] - print(f"Number of objects with receptacles = {len(all_handles)}") - # ---------------------------------------------------- - # load object orientation metadata - reorient_objects = False - if reorient_objects: - fp_models_metadata_file = ( - "/home/alexclegg/Documents/dev/fphab/fpModels_metadata.csv" - ) - obj_orientations = parse_object_orientations_from_metadata_csv( - fp_models_metadata_file + for scene_of_interest in scenes_of_interest: + cpo.init_caches() + # ---------------------------------------------------- + # get object handles from a specific scene + objects_in_scene = get_objects_in_scene( + dataset_path=args.dataset, scene_handle=scene_of_interest, mm=cpo.mm ) - correct_object_orientations(all_handles, obj_orientations, cpo.mm) - # ---------------------------------------------------- - - print(f"all_handles[0] = {all_handles[0]}") - - # ---------------------------------------------------- - - # ---------------------------------------------------- - # NOTE: select objects for testing VHACD pipeline - # target_object_handles = [ - # "01be253cbfd14b947e9dbe09d0b1959e97d72122", # desk - # "01b65339d622bb9f89eb8fdd753a76cffc7eb8d6", # shelves, - # "00ea83bf1b2544df87f6d52d02382c0bb75598c6", # bookcase - # "00e388a751b3654216f2109ee073dc44f1241eee", # counter - # "01d9fff2f701af7d5d40a7a5adad5bf40d4c49c8", # round table - # "03c328fccef4975310314838e42b6dff06709b06", # shelves - # "0110c7ff0e787bf98c9da923554ddea1484e4a3d", # wood table - # "00366b86401aa16b702c21de49fd59b75ab9c57b", # ratan sofa - # ] - # all_handles = [ - # h for h in all_handles if any([t in h for t in target_object_handles]) - # ] - # ---------------------------------------------------- - - # ---------------------------------------------------- - # indexed subset of the objects - # all_handles = all_handles[1:2] - # ---------------------------------------------------- + all_handles = objects_in_scene + # ---------------------------------------------------- + + # ---------------------------------------------------- + # get a subset with receptacles defined + all_handles = [ + all_handles[i] + for i in range(len(all_handles)) + if object_has_receptacles(all_handles[i], otm) + ] + # all_handles = [all_handles[0]] + print(f"Number of objects with receptacles = {len(all_handles)}") + # ---------------------------------------------------- + + # ---------------------------------------------------- + # load object orientation metadata + reorient_objects = False + if reorient_objects: + fp_models_metadata_file = ( + "/home/alexclegg/Documents/dev/fphab/fpModels_metadata.csv" + ) + obj_orientations = parse_object_orientations_from_metadata_csv( + fp_models_metadata_file + ) + correct_object_orientations(all_handles, obj_orientations, cpo.mm) + # ---------------------------------------------------- - # print(all_handles) - cpo.compute_and_save_results_for_objects(all_handles) + cpo.compute_and_save_results_for_objects( + all_handles, output_filename=scene_of_interest + "_cpo_out" + ) if __name__ == "__main__": From d1124e230eb0b24770f24261978fb003aed36ba0 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 14 Apr 2023 08:10:35 -0700 Subject: [PATCH 027/153] use CollisionProxyOptimizer in viewer tool and remove outdated evaluate_collision_shape func. output_dir is now optional --- examples/viewer.py | 50 +----- tools/collision_shape_automation.py | 238 +++++++--------------------- 2 files changed, 65 insertions(+), 223 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 90c50754e8..e1f5ee0195 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -36,7 +36,6 @@ pr_raycast_results = None obj_temp_handle = None test_points = None -normalized_error = 0 class HabitatSimInteractiveViewer(Application): @@ -215,7 +214,6 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: LoggingContext.reinitialize_from_env() logger.setLevel("INFO") self.print_help_text() - print(f"Normalized Error = {normalized_error}") def draw_contact_debug(self, debug_line_render: Any): """ @@ -1436,46 +1434,14 @@ def next_frame() -> None: mm = habitat_sim.metadata.MetadataMediator() mm.active_dataset = sim_settings["scene_dataset_config_file"] - num_point_samples = 100 - sample_metrics = [] - avg_metric = 0 - avg_profile_metrics = {} - for _sample in range(10): - ( - gt_raycast_results, - pr_raycast_results, - obj_temp_handle, - test_points, - normalized_error, - profile_metrics, - ) = csa.evaluate_collision_shape( - obj_name, - sim_settings, - sample_shape="jittered_aabb", - mm=mm, - num_point_samples=num_point_samples, - ) - for key in list(profile_metrics.keys()): - if key not in avg_profile_metrics: - avg_profile_metrics[key] = profile_metrics[key] - else: - avg_profile_metrics[key] += profile_metrics[key] - - sample_metrics.append(normalized_error) - avg_metric += normalized_error - for key in list(avg_profile_metrics.keys()): - avg_profile_metrics[key] = avg_profile_metrics[key] / len(sample_metrics) - avg_metric /= len(sample_metrics) - print(f"sample_metrics ({len(sample_metrics)} samples) = {sample_metrics}") - print(f"avg_metric = {avg_metric}") - variance = 0 - for metric in sample_metrics: - variance += (metric - avg_metric) ** 2 - variance /= len(sample_metrics) - 1 - print(f"variance = {variance}") - print(f"std = {math.sqrt(variance)}") - print(f"avg_profile_metrics = {avg_profile_metrics}") - # exit() + cpo = csa.CollisionProxyOptimizer(sim_settings) + obj_temp_handle = mm.object_template_manager.get_file_template_handles(obj_name)[0] + cpo.setup_obj_gt(obj_temp_handle) + cpo.compute_proxy_metrics(obj_temp_handle) + # setup globals for debug drawing + test_points = cpo.gt_data[obj_temp_handle]["test_points"] + pr_raycast_results = cpo.gt_data[obj_temp_handle]["raycasts"]["pr0"] + gt_raycast_results = cpo.gt_data[obj_temp_handle]["raycasts"]["gt"] # start the application HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 155edfa6bf..9931168b25 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -8,7 +8,7 @@ import os import random import time -from typing import Any, Dict, List, Tuple +from typing import Any, Dict, List, Optional, Tuple # imports from Habitat-lab # NOTE: requires PR 1108 branch: rearrange-gen-improvements (https://github.com/facebookresearch/habitat-lab/pull/1108) @@ -561,147 +561,6 @@ def get_raycast_results_cumulative_error_metric( return normalized_error -def evaluate_collision_shape( - object_handle: str, - sim_settings: Dict[str, Any], - sample_shape="sphere", - mm=None, - num_point_samples=100, -) -> None: - """ - Runs in-engine evaluation of collision shape accuracy for a single object. - Uses a raycast from a bounding shape to approximate surface error between a proxy shape and ground truth (the render shape). - Returns: - ground_truth and proxy raw raycast results, - the object's template handle, - all test points used for the raycast, - scalar error metrics - 1. initializes a simulator with the object render shape as a stage collision mesh. - 2. uses the scene bouding box to sample points on a configured bounding shape (e.g. inflated AABB or sphere). - 3. raycasts between sampled point pairs on both ground truth and collision proxy shapes to heuristicall measure error. - 4. computes scalar error metrics - - :param object_handle: The object to evaluate. - :param sim_settings: Any simulator settings for initialization (should be physics enabled and point to correct dataset). - :param sample_shape: The desired bounding shape for raycast: "sphere" or "aabb", "jittered_aabb". - :param mm: A pre-configured MetadataMediator may be provided to reduce initialization time. Should have the correct dataset already configured. - """ - profile_metrics = {} - start_time = time.time() - check_time = time.time() - cfg = make_cfg(sim_settings) - if mm is not None: - cfg.metadata_mediator = mm - with habitat_sim.Simulator(cfg) as sim: - profile_metrics["init0"] = time.time() - check_time - check_time = time.time() - # evaluate the collision shape defined in an object's template - # 1. get the template from MM - matching_templates = sim.get_object_template_manager().get_template_handles( - object_handle - ) - assert ( - len(matching_templates) == 1 - ), f"Multiple or no template matches for handle '{object_handle}': ({matching_templates})" - obj_template_handle = matching_templates[0] - obj_template = sim.get_object_template_manager().get_template_by_handle( - obj_template_handle - ) - obj_template.compute_COM_from_shape = False - obj_template.com = mn.Vector3(0) - # obj_template.bounding_box_collisions = True - # obj_template.is_collidable = False - sim.get_object_template_manager().register_template(obj_template) - # 2. Setup a stage from the object's render mesh - stm = sim.get_stage_template_manager() - stage_template_name = "obj_as_stage_template" - new_stage_template = stm.create_new_template(handle=stage_template_name) - new_stage_template.render_asset_handle = obj_template.render_asset_handle - new_stage_template.orient_up = obj_template.orient_up - new_stage_template.orient_front = obj_template.orient_front - stm.register_template( - template=new_stage_template, specified_handle=stage_template_name - ) - # 3. Initialize the simulator for the stage - new_settings = sim_settings.copy() - new_settings["scene"] = stage_template_name - new_config = make_cfg(new_settings) - sim.reconfigure(new_config) - profile_metrics["init_stage"] = time.time() - check_time - check_time = time.time() - - # 4. compute initial metric baselines - scene_bb = sim.get_active_scene_graph().get_root_node().cumulative_bb - inflated_scene_bb = scene_bb.scaled(mn.Vector3(1.25)) - inflated_scene_bb = mn.Range3D.from_center( - scene_bb.center(), inflated_scene_bb.size() / 2.0 - ) - test_points = None - if sample_shape == "aabb": - # bounding box sample - test_points = sample_points_from_range3d( - range3d=inflated_scene_bb, num_points=num_point_samples - ) - elif sample_shape == "jittered_aabb": - # bounding box sample - test_points = sample_jittered_points_from_range3d( - range3d=inflated_scene_bb, num_points=num_point_samples - ) - elif sample_shape == "sphere": - # bounding sphere sample - half_diagonal = (scene_bb.max - scene_bb.min).length() / 2.0 - test_points = sample_points_from_sphere( - center=inflated_scene_bb.center(), - radius=half_diagonal, - num_points=num_point_samples, - ) - else: - raise NotImplementedError( - f"sample_shape == `{sample_shape}` is not implemented. Use `sphere` or `aabb`." - ) - profile_metrics["sample_points"] = time.time() - check_time - check_time = time.time() - - print("GT raycast:") - gt_raycast_results = run_pairwise_raycasts(test_points, sim) - profile_metrics["raycast_stage"] = time.time() - check_time - check_time = time.time() - - # 5. load the object with proxy (in NONE stage) - new_settings = sim_settings.copy() - new_config = make_cfg(new_settings) - sim.reconfigure(new_config) - obj = sim.get_rigid_object_manager().add_object_by_template_handle( - obj_template_handle - ) - obj.translation = obj.com - profile_metrics["init_object"] = time.time() - check_time - check_time = time.time() - - # 6. compute the metric for proxy object - print("PR raycast:") - pr_raycast_results = run_pairwise_raycasts(test_points, sim) - profile_metrics["raycast_object"] = time.time() - check_time - check_time = time.time() - - # 7. compare metrics - normalized_error = get_raycast_results_cumulative_error_metric( - gt_raycast_results, pr_raycast_results - ) - profile_metrics["compute_metrics"] = time.time() - check_time - check_time = time.time() - profile_metrics["total"] = time.time() - start_time - - return ( - gt_raycast_results, - pr_raycast_results, - obj_template_handle, - test_points, - normalized_error, - profile_metrics, - ) - - # =================================================================== # CollisionProxyOptimizer class provides a stateful API for # configurable evaluation and optimization of collision proxy shapes. @@ -713,15 +572,21 @@ class CollisionProxyOptimizer: Stateful control flow for using Habitat-sim to evaluate and optimize collision proxy shapes. """ - def __init__(self, sim_settings: Dict[str, Any], output_directory="") -> None: + def __init__( + self, + sim_settings: Dict[str, Any], + output_directory: Optional[str] = None, + mm: Optional[habitat_sim.metadata.MetadataMediator] = None, + ) -> None: # load the dataset into a persistent, shared MetadataMediator instance. - self.mm = habitat_sim.metadata.MetadataMediator() + self.mm = mm if mm is not None else habitat_sim.metadata.MetadataMediator() self.mm.active_dataset = sim_settings["scene_dataset_config_file"] self.sim_settings = sim_settings.copy() # path to the desired output directory for images/csv self.output_directory = output_directory - os.makedirs(self.output_directory, exist_ok=True) + if output_directory is not None: + os.makedirs(self.output_directory, exist_ok=True) # if true, render and save debug images in self.output_directory self.generate_debug_images = False @@ -893,21 +758,22 @@ def setup_obj_gt( ) ) ) - # use DebugVisualizer to get 6-axis view of the object - dvb = hab_debug_vis.DebugVisualizer( - sim=sim, - output_path=self.output_directory, - default_sensor_uuid="color_sensor", - ) - dvb.peek_rigid_object( - obj, - peek_all_axis=True, - additional_savefile_prefix=f"{receptacle.name}_", - debug_lines=debug_lines, - debug_circles=debug_circles, - ) + if self.output_directory is not None: + # use DebugVisualizer to get 6-axis view of the object + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + dvb.peek_rigid_object( + obj, + peek_all_axis=True, + additional_savefile_prefix=f"{receptacle.name}_", + debug_lines=debug_lines, + debug_circles=debug_circles, + ) - if self.generate_debug_images: + if self.generate_debug_images and self.output_directory is not None: # use DebugVisualizer to get 6-axis view of the object dvb = hab_debug_vis.DebugVisualizer( sim=sim, @@ -1050,7 +916,7 @@ def compute_proxy_metrics(self, obj_handle: str) -> None: col_bb.max ), f"Inflated bounding box does not contain the collision shape. (Object `{obj_handle}`)" - if self.generate_debug_images: + if self.generate_debug_images and self.output_directory is not None: # use DebugVisualizer to get 6-axis view of the object dvb = hab_debug_vis.DebugVisualizer( sim=sim, @@ -1124,11 +990,13 @@ def compute_receptacle_access_metrics( # save a list of point accessibility scores for debugging and visualization receptacle_point_access_scores = {} - dvb = hab_debug_vis.DebugVisualizer( - sim=sim, - output_path=self.output_directory, - default_sensor_uuid="color_sensor", - ) + dvb: Optional[hab_debug_vis.DebugVisualizer] = None + if self.output_directory is not None: + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) # collect hemisphere raycast samples for all receptacle sample points for receptacle_name in obj_rec_data.keys(): @@ -1190,7 +1058,7 @@ def compute_receptacle_access_metrics( print(f" receptacle_access_score = {receptacle_access_score}") print(f" receptacle_access_rate = {receptacle_access_rate}") - if self.generate_debug_images: + if self.generate_debug_images and dvb is not None: # generate receptacle access debug images # 1a Show missed rays vs 1b hit rays debug_lines = [] @@ -1305,11 +1173,13 @@ def compute_receptacle_stability( scene_name = self.gt_data[obj_handle]["stage_template_name"] cfg = self.get_cfg_with_mm(scene=scene_name) with habitat_sim.Simulator(cfg) as sim: - dvb = hab_debug_vis.DebugVisualizer( - sim=sim, - output_path=self.output_directory, - default_sensor_uuid="color_sensor", - ) + dvb: Optional[hab_debug_vis.DebugVisualizer] = None + if self.output_directory is not None: + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) # load the object rom = sim.get_rigid_object_manager() obj = None @@ -1468,16 +1338,18 @@ def run_physics_settle_test(self, obj_handle): plane_obj.motion_type = habitat_sim.physics.MotionType.STATIC # use DebugVisualizer to get 6-axis view of the object - dvb = hab_debug_vis.DebugVisualizer( - sim=sim, - output_path=self.output_directory, - default_sensor_uuid="color_sensor", - ) - dvb.peek_rigid_object( - obj, - peek_all_axis=True, - additional_savefile_prefix=f"plane_snap_{shape_id}_", - ) + dvb: Optional[hab_debug_vis.DebugVisualizer] = None + if self.output_directory is not None: + dvb = hab_debug_vis.DebugVisualizer( + sim=sim, + output_path=self.output_directory, + default_sensor_uuid="color_sensor", + ) + dvb.peek_rigid_object( + obj, + peek_all_axis=True, + additional_savefile_prefix=f"plane_snap_{shape_id}_", + ) # snap the object to the plane obj_col_bb = obj.collision_shape_aabb @@ -1949,6 +1821,10 @@ def save_results_to_csv(self, filename: str) -> None: assert len(self.results) > 0, "There must be results to save." + assert ( + self.output_directory is not None + ), "Must have an output directory to save." + import csv filepath = os.path.join(self.output_directory, filename) From 494299710e3173a198dee1921122a5d8a3fd1e05 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 14 Apr 2023 14:16:00 -0700 Subject: [PATCH 028/153] separate obj_viewer and (scene)viewer. Add scene viewer features to visualize receptacles and collision objects. --- examples/obj_viewer.py | 1384 ++++++++++++++++++++++++++++++++++++++++ examples/viewer.py | 236 ++----- 2 files changed, 1426 insertions(+), 194 deletions(-) create mode 100644 examples/obj_viewer.py diff --git a/examples/obj_viewer.py b/examples/obj_viewer.py new file mode 100644 index 0000000000..561b853944 --- /dev/null +++ b/examples/obj_viewer.py @@ -0,0 +1,1384 @@ +# 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 ctypes +import math +import os +import string +import sys +import time +from enum import Enum +from typing import Any, Callable, Dict, List, Optional, Tuple + +flags = sys.getdlopenflags() +sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) + +import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle +import magnum as mn +import numpy as np +from habitat.sims.habitat_simulator.sim_utilities import snap_down +from magnum import shaders, text +from magnum.platform.glfw import Application + +import habitat_sim +from habitat_sim import ReplayRenderer, ReplayRendererConfiguration, physics +from habitat_sim.logging import LoggingContext, logger +from habitat_sim.utils.common import d3_40_colors_rgb, quat_from_angle_axis +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +# add tools directory so I can import things to try them in the viewer +sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../tools")) +print(sys.path) +import collision_shape_automation as csa + +gt_raycast_results = None +pr_raycast_results = None +obj_temp_handle = None +test_points = None + + +class HabitatSimInteractiveViewer(Application): + # the maximum number of chars displayable in the app window + # using the magnum text module. These chars are used to + # display the CPU/GPU usage data + MAX_DISPLAY_TEXT_CHARS = 256 + + # how much to displace window text relative to the center of the + # app window (e.g if you want the display text in the top left of + # the app window, you will displace the text + # window width * -TEXT_DELTA_FROM_CENTER in the x axis and + # window height * TEXT_DELTA_FROM_CENTER in the y axis, as the text + # position defaults to the middle of the app window) + TEXT_DELTA_FROM_CENTER = 0.49 + + # font size of the magnum in-window display text that displays + # CPU and GPU usage info + DISPLAY_FONT_SIZE = 16.0 + + def __init__(self, sim_settings: Dict[str, Any]) -> None: + self.sim_settings: Dict[str:Any] = sim_settings + + self.enable_batch_renderer: bool = self.sim_settings["enable_batch_renderer"] + self.num_env: int = ( + self.sim_settings["num_environments"] if self.enable_batch_renderer else 1 + ) + + # Compute environment camera resolution based on the number of environments to render in the window. + window_size: mn.Vector2 = ( + self.sim_settings["window_width"], + self.sim_settings["window_height"], + ) + + configuration = self.Configuration() + configuration.title = "Habitat Sim Interactive Viewer" + configuration.size = window_size + Application.__init__(self, configuration) + self.fps: float = 60.0 + + # Compute environment camera resolution based on the number of environments to render in the window. + grid_size: mn.Vector2i = ReplayRenderer.environment_grid_size(self.num_env) + camera_resolution: mn.Vector2 = mn.Vector2(self.framebuffer_size) / mn.Vector2( + grid_size + ) + self.sim_settings["width"] = camera_resolution[0] + self.sim_settings["height"] = camera_resolution[1] + + # draw Bullet debug line visualizations (e.g. collision meshes) + self.debug_bullet_draw = False + # draw active contact point debug line visualizations + self.contact_debug_draw = False + # cache most recently loaded URDF file for quick-reload + self.cached_urdf = "" + + # set up our movement map + key = Application.KeyEvent.Key + self.pressed = { + key.UP: False, + key.DOWN: False, + key.LEFT: False, + key.RIGHT: False, + key.A: False, + key.D: False, + key.S: False, + key.W: False, + key.X: False, + key.Z: False, + } + + # set up our movement key bindings map + key = Application.KeyEvent.Key + self.key_to_action = { + key.UP: "look_up", + key.DOWN: "look_down", + key.LEFT: "turn_left", + key.RIGHT: "turn_right", + key.A: "move_left", + key.D: "move_right", + key.S: "move_backward", + key.W: "move_forward", + key.X: "move_down", + key.Z: "move_up", + } + + # Load a TrueTypeFont plugin and open the font file + self.display_font = text.FontManager().load_and_instantiate("TrueTypeFont") + relative_path_to_font = "../data/fonts/ProggyClean.ttf" + self.display_font.open_file( + os.path.join(os.path.dirname(__file__), relative_path_to_font), + 13, + ) + + # Glyphs we need to render everything + self.glyph_cache = text.GlyphCache(mn.Vector2i(256)) + self.display_font.fill_glyph_cache( + self.glyph_cache, + string.ascii_lowercase + + string.ascii_uppercase + + string.digits + + ":-_+,.! %µ", + ) + + # magnum text object that displays CPU/GPU usage data in the app window + self.window_text = text.Renderer2D( + self.display_font, + self.glyph_cache, + HabitatSimInteractiveViewer.DISPLAY_FONT_SIZE, + text.Alignment.TOP_LEFT, + ) + self.window_text.reserve(HabitatSimInteractiveViewer.MAX_DISPLAY_TEXT_CHARS) + + # text object transform in window space is Projection matrix times Translation Matrix + # put text in top left of window + self.window_text_transform = mn.Matrix3.projection( + self.framebuffer_size + ) @ mn.Matrix3.translation( + mn.Vector2(self.framebuffer_size) + * mn.Vector2( + -HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + ) + ) + self.shader = shaders.VectorGL2D() + + # make magnum text background transparent + mn.gl.Renderer.enable(mn.gl.Renderer.Feature.BLENDING) + mn.gl.Renderer.set_blend_function( + mn.gl.Renderer.BlendFunction.ONE, + mn.gl.Renderer.BlendFunction.ONE_MINUS_SOURCE_ALPHA, + ) + mn.gl.Renderer.set_blend_equation( + mn.gl.Renderer.BlendEquation.ADD, mn.gl.Renderer.BlendEquation.ADD + ) + + # variables that track app data and CPU/GPU usage + self.num_frames_to_track = 60 + + # Cycle mouse utilities + self.mouse_interaction = MouseMode.LOOK + self.mouse_grabber: Optional[MouseGrabber] = None + self.previous_mouse_point = None + + # toggle physics simulation on/off + self.simulating = False + self.sample_seed = 0 + self.collision_proxy_obj = None + self.mouse_cast_results = None + self.debug_draw_raycasts = True + + self.debug_draw_receptacles = True + self.object_receptacles = [] + + # toggle a single simulation step at the next opportunity if not + # simulating continuously. + self.simulate_single_step = False + + # configure our simulator + self.cfg: Optional[habitat_sim.simulator.Configuration] = None + self.sim: Optional[habitat_sim.simulator.Simulator] = None + self.tiled_sims: list[habitat_sim.simulator.Simulator] = None + self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None + self.replay_renderer: Optional[ReplayRenderer] = None + self.reconfigure_sim() + + if self.sim.pathfinder.is_loaded: + self.sim.pathfinder = habitat_sim.nav.PathFinder() + + # compute NavMesh if not already loaded by the scene. + # if ( + # not self.sim.pathfinder.is_loaded + # and self.cfg.sim_cfg.scene_id.lower() != "none" + # ): + # self.navmesh_config_and_recompute() + + self.time_since_last_simulation = 0.0 + LoggingContext.reinitialize_from_env() + logger.setLevel("INFO") + self.print_help_text() + + def draw_contact_debug(self): + """ + This method is called to render a debug line overlay displaying active contact points and normals. + Yellow lines show the contact distance along the normal and red lines show the contact normal at a fixed length. + """ + yellow = mn.Color4.yellow() + red = mn.Color4.red() + cps = self.sim.get_physics_contact_points() + self.sim.get_debug_line_render().set_line_width(1.5) + camera_position = self.render_camera.render_camera.node.absolute_translation + # only showing active contacts + active_contacts = (x for x in cps if x.is_active) + for cp in active_contacts: + # red shows the contact distance + self.sim.get_debug_line_render().draw_transformed_line( + cp.position_on_b_in_ws, + cp.position_on_b_in_ws + + cp.contact_normal_on_b_in_ws * -cp.contact_distance, + red, + ) + # yellow shows the contact normal at a fixed length for visualization + self.sim.get_debug_line_render().draw_transformed_line( + cp.position_on_b_in_ws, + # + cp.contact_normal_on_b_in_ws * cp.contact_distance, + cp.position_on_b_in_ws + cp.contact_normal_on_b_in_ws * 0.1, + yellow, + ) + self.sim.get_debug_line_render().draw_circle( + translation=cp.position_on_b_in_ws, + radius=0.005, + color=yellow, + normal=camera_position - cp.position_on_b_in_ws, + ) + + def debug_draw(self): + """ + Additional draw commands to be called during draw_event. + """ + if self.debug_bullet_draw: + render_cam = self.render_camera.render_camera + proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) + self.sim.physics_debug_draw(proj_mat) + if self.contact_debug_draw: + self.draw_contact_debug() + + # mouse raycast circle + white = mn.Color4(mn.Vector3(1.0), 1.0) + if self.mouse_cast_results is not None and self.mouse_cast_results.has_hits(): + m_ray = self.mouse_cast_results.ray + self.sim.get_debug_line_render().draw_circle( + translation=m_ray.origin + + m_ray.direction + * self.mouse_cast_results.hits[0].ray_distance + * m_ray.direction.length(), + radius=0.005, + color=white, + normal=self.mouse_cast_results.hits[0].normal, + ) + + if gt_raycast_results is not None and self.debug_draw_raycasts: + scene_bb = self.sim.get_active_scene_graph().get_root_node().cumulative_bb + inflated_scene_bb = scene_bb.scaled(mn.Vector3(1.25)) + inflated_scene_bb = mn.Range3D.from_center( + scene_bb.center(), inflated_scene_bb.size() / 2.0 + ) + self.sim.get_debug_line_render().draw_box( + inflated_scene_bb.min, inflated_scene_bb.max, white + ) + if self.sim.get_rigid_object_manager().get_num_objects() == 0: + self.collision_proxy_obj = ( + self.sim.get_rigid_object_manager().add_object_by_template_handle( + obj_temp_handle + ) + ) + self.collision_proxy_obj.motion_type = ( + habitat_sim.physics.MotionType.KINEMATIC + ) + + csa.debug_draw_raycast_results( + self.sim, gt_raycast_results, pr_raycast_results, seed=self.sample_seed + ) + + # draw test points + for side in test_points: + for p in side: + self.sim.get_debug_line_render().draw_circle( + translation=p, + radius=0.005, + color=mn.Color4.magenta(), + ) + + if self.debug_draw_receptacles and self.collision_proxy_obj is not None: + # parse any receptacles defined for the object + if len(self.object_receptacles) == 0: + source_template_file = ( + self.collision_proxy_obj.creation_attributes.file_directory + ) + user_attr = self.collision_proxy_obj.user_attributes + self.object_receptacles = ( + hab_receptacle.parse_receptacles_from_user_config( + user_attr, + parent_object_handle=self.collision_proxy_obj.handle, + parent_template_directory=source_template_file, + ) + ) + # draw any receptacles for the object + for rix, receptacle in enumerate(self.object_receptacles): + c = d3_40_colors_rgb[rix] + rec_color = mn.Vector3(c[0], c[1], c[2]) / 256.0 + receptacle.debug_draw(self.sim, color=mn.Color4(rec_color)) + + def draw_event( + self, + simulation_call: Optional[Callable] = None, + global_call: Optional[Callable] = None, + active_agent_id_and_sensor_name: Tuple[int, str] = (0, "color_sensor"), + ) -> None: + """ + Calls continuously to re-render frames and swap the two frame buffers + at a fixed rate. + """ + agent_acts_per_sec = self.fps + + mn.gl.default_framebuffer.clear( + mn.gl.FramebufferClear.COLOR | mn.gl.FramebufferClear.DEPTH + ) + + # Agent actions should occur at a fixed rate per second + self.time_since_last_simulation += Timer.prev_frame_duration + num_agent_actions: int = self.time_since_last_simulation * agent_acts_per_sec + self.move_and_look(int(num_agent_actions)) + + # Occasionally a frame will pass quicker than 1/60 seconds + if self.time_since_last_simulation >= 1.0 / self.fps: + if self.simulating or self.simulate_single_step: + self.sim.step_world(1.0 / self.fps) + self.simulate_single_step = False + if simulation_call is not None: + simulation_call() + if global_call is not None: + global_call() + + # reset time_since_last_simulation, accounting for potential overflow + self.time_since_last_simulation = math.fmod( + self.time_since_last_simulation, 1.0 / self.fps + ) + + keys = active_agent_id_and_sensor_name + + if self.enable_batch_renderer: + self.render_batch() + else: + self.sim._Simulator__sensors[keys[0]][keys[1]].draw_observation() + agent = self.sim.get_agent(keys[0]) + self.render_camera = agent.scene_node.node_sensor_suite.get(keys[1]) + self.debug_draw() + self.render_camera.render_target.blit_rgba_to_default() + + # draw CPU/GPU usage data and other info to the app window + mn.gl.default_framebuffer.bind() + self.draw_text(self.render_camera.specification()) + + self.swap_buffers() + Timer.next_frame() + self.redraw() + + def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: + """ + Set up our own agent and agent controls + """ + make_action_spec = habitat_sim.agent.ActionSpec + make_actuation_spec = habitat_sim.agent.ActuationSpec + MOVE, LOOK = 0.07, 1.5 + + # all of our possible actions' names + action_list = [ + "move_left", + "turn_left", + "move_right", + "turn_right", + "move_backward", + "look_up", + "move_forward", + "look_down", + "move_down", + "move_up", + ] + + action_space: Dict[str, habitat_sim.agent.ActionSpec] = {} + + # build our action space map + for action in action_list: + actuation_spec_amt = MOVE if "move" in action else LOOK + action_spec = make_action_spec( + action, make_actuation_spec(actuation_spec_amt) + ) + action_space[action] = action_spec + + sensor_spec: List[habitat_sim.sensor.SensorSpec] = self.cfg.agents[ + self.agent_id + ].sensor_specifications + + agent_config = habitat_sim.agent.AgentConfiguration( + height=1.5, + radius=0.1, + sensor_specifications=sensor_spec, + action_space=action_space, + body_type="cylinder", + ) + return agent_config + + def reconfigure_sim(self) -> None: + """ + Utilizes the current `self.sim_settings` to configure and set up a new + `habitat_sim.Simulator`, and then either starts a simulation instance, or replaces + the current simulator instance, reloading the most recently loaded scene + """ + # configure our sim_settings but then set the agent to our default + self.cfg = make_cfg(self.sim_settings) + self.agent_id: int = self.sim_settings["default_agent"] + self.cfg.agents[self.agent_id] = self.default_agent_config() + + if self.enable_batch_renderer: + self.cfg.enable_batch_renderer = True + self.cfg.sim_cfg.create_renderer = False + self.cfg.sim_cfg.enable_gfx_replay_save = True + + if self.sim_settings["stage_requires_lighting"]: + logger.info("Setting synthetic lighting override for stage.") + self.cfg.sim_cfg.override_scene_light_defaults = True + self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY + + # create custom stage from object + self.cfg.metadata_mediator = habitat_sim.metadata.MetadataMediator() + self.cfg.metadata_mediator.active_dataset = self.sim_settings[ + "scene_dataset_config_file" + ] + if args.reorient_object: + obj_handle = ( + self.cfg.metadata_mediator.object_template_manager.get_template_handles( + args.scene + )[0] + ) + fp_models_metadata_file = ( + "/home/alexclegg/Documents/dev/fphab/fpModels_metadata.csv" + ) + obj_orientations = csa.parse_object_orientations_from_metadata_csv( + fp_models_metadata_file + ) + csa.correct_object_orientations( + [obj_handle], obj_orientations, self.cfg.metadata_mediator + ) + + otm = self.cfg.metadata_mediator.object_template_manager + obj_template = otm.get_template_by_handle(obj_temp_handle) + obj_template.compute_COM_from_shape = False + obj_template.com = mn.Vector3(0) + otm.register_template(obj_template) + stm = self.cfg.metadata_mediator.stage_template_manager + stage_template_name = "obj_as_stage_template" + new_stage_template = stm.create_new_template(handle=stage_template_name) + new_stage_template.render_asset_handle = obj_template.render_asset_handle + new_stage_template.orient_up = obj_template.orient_up + new_stage_template.orient_front = obj_template.orient_front + + # margin must be 0 for snapping to work on overlapped gt/proxy + new_stage_template.margin = 0.0 + stm.register_template( + template=new_stage_template, specified_handle=stage_template_name + ) + self.cfg.sim_cfg.scene_id = stage_template_name + # visualize the object as its collision shape + obj_template.render_asset_handle = obj_template.collision_asset_handle + otm.register_template(obj_template) + + if self.sim is None: + self.tiled_sims = [] + for _i in range(self.num_env): + self.tiled_sims.append(habitat_sim.Simulator(self.cfg)) + self.sim = self.tiled_sims[0] + else: # edge case + for i in range(self.num_env): + if ( + self.tiled_sims[i].config.sim_cfg.scene_id + == self.cfg.sim_cfg.scene_id + ): + # we need to force a reset, so change the internal config scene name + self.tiled_sims[i].config.sim_cfg.scene_id = "NONE" + self.tiled_sims[i].reconfigure(self.cfg) + + # post reconfigure + self.default_agent = self.sim.get_agent(self.agent_id) + self.render_camera = self.default_agent.scene_node.node_sensor_suite.get( + "color_sensor" + ) + + # set sim_settings scene name as actual loaded scene + self.sim_settings["scene"] = self.sim.curr_scene_name + + # Initialize replay renderer + if self.enable_batch_renderer and self.replay_renderer is None: + self.replay_renderer_cfg = ReplayRendererConfiguration() + self.replay_renderer_cfg.num_environments = self.num_env + self.replay_renderer_cfg.standalone = ( + False # Context is owned by the GLFW window + ) + self.replay_renderer_cfg.sensor_specifications = self.cfg.agents[ + self.agent_id + ].sensor_specifications + self.replay_renderer_cfg.gpu_device_id = self.cfg.sim_cfg.gpu_device_id + self.replay_renderer_cfg.force_separate_semantic_scene_graph = False + self.replay_renderer_cfg.leave_context_with_background_renderer = False + self.replay_renderer = ReplayRenderer.create_batch_replay_renderer( + self.replay_renderer_cfg + ) + # Pre-load composite files + if sim_settings["composite_files"] is not None: + for composite_file in sim_settings["composite_files"]: + self.replay_renderer.preload_file(composite_file) + + Timer.start() + self.step = -1 + + def render_batch(self): + """ + This method updates the replay manager with the current state of environments and renders them. + """ + for i in range(self.num_env): + # Apply keyframe + keyframe = self.tiled_sims[i].gfx_replay_manager.extract_keyframe() + self.replay_renderer.set_environment_keyframe(i, keyframe) + # Copy sensor transforms + sensor_suite = self.tiled_sims[i]._sensors + for sensor_uuid, sensor in sensor_suite.items(): + transform = sensor._sensor_object.node.absolute_transformation() + self.replay_renderer.set_sensor_transform(i, sensor_uuid, transform) + # Render + self.replay_renderer.render(mn.gl.default_framebuffer) + + def move_and_look(self, repetitions: int) -> None: + """ + This method is called continuously with `self.draw_event` to monitor + any changes in the movement keys map `Dict[KeyEvent.key, Bool]`. + When a key in the map is set to `True` the corresponding action is taken. + """ + # avoids unnecessary updates to grabber's object position + if repetitions == 0: + return + + key = Application.KeyEvent.Key + agent = self.sim.agents[self.agent_id] + press: Dict[key.key, bool] = self.pressed + act: Dict[key.key, str] = self.key_to_action + + action_queue: List[str] = [act[k] for k, v in press.items() if v] + + for _ in range(int(repetitions)): + [agent.act(x) for x in action_queue] + + # update the grabber transform when our agent is moved + if self.mouse_grabber is not None: + # update location of grabbed object + self.update_grab_position(self.previous_mouse_point) + + def invert_gravity(self) -> None: + """ + Sets the gravity vector to the negative of it's previous value. This is + a good method for testing simulation functionality. + """ + gravity: mn.Vector3 = self.sim.get_gravity() * -1 + self.sim.set_gravity(gravity) + + def key_press_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key press by performing the corresponding functions. + If the key pressed is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the + key will be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + pressed = Application.KeyEvent.Key + mod = Application.InputEvent.Modifier + + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) + # warning: ctrl doesn't always pass through with other key-presses + + if key == pressed.ESC: + event.accepted = True + self.exit_event(Application.ExitEvent) + return + + elif key == pressed.H: + self.print_help_text() + + elif key == pressed.TAB: + # NOTE: (+ALT) - reconfigure without cycling scenes + if not alt_pressed: + # cycle the active scene from the set available in MetadataMediator + inc = -1 if shift_pressed else 1 + scene_ids = self.sim.metadata_mediator.get_scene_handles() + cur_scene_index = 0 + if self.sim_settings["scene"] not in scene_ids: + matching_scenes = [ + (ix, x) + for ix, x in enumerate(scene_ids) + if self.sim_settings["scene"] in x + ] + if not matching_scenes: + logger.warning( + f"The current scene, '{self.sim_settings['scene']}', is not in the list, starting cycle at index 0." + ) + else: + cur_scene_index = matching_scenes[0][0] + else: + cur_scene_index = scene_ids.index(self.sim_settings["scene"]) + + next_scene_index = min( + max(cur_scene_index + inc, 0), len(scene_ids) - 1 + ) + self.sim_settings["scene"] = scene_ids[next_scene_index] + self.reconfigure_sim() + logger.info( + f"Reconfigured simulator for scene: {self.sim_settings['scene']}" + ) + + elif key == pressed.SPACE: + if not self.sim.config.sim_cfg.enable_physics: + logger.warn("Warning: physics was not enabled during setup") + else: + self.simulating = not self.simulating + logger.info(f"Command: physics simulating set to {self.simulating}") + + elif key == pressed.PERIOD: + if self.simulating: + logger.warn("Warning: physics simulation already running") + else: + self.simulate_single_step = True + logger.info("Command: physics step taken") + + elif key == pressed.COMMA: + self.debug_bullet_draw = not self.debug_bullet_draw + logger.info(f"Command: toggle Bullet debug draw: {self.debug_bullet_draw}") + + elif key == pressed.C: + if shift_pressed: + self.contact_debug_draw = not self.contact_debug_draw + logger.info( + f"Command: toggle contact debug draw: {self.contact_debug_draw}" + ) + else: + # perform a discrete collision detection pass and enable contact debug drawing to visualize the results + logger.info( + "Command: perform discrete collision detection and visualize active contacts." + ) + self.sim.perform_discrete_collision_detection() + self.contact_debug_draw = True + # TODO: add a nice log message with concise contact pair naming. + + elif key == pressed.O: + # move the object in/out of the frame + if self.collision_proxy_obj is not None: + if self.collision_proxy_obj.translation == mn.Vector3(0): + self.collision_proxy_obj.translation = mn.Vector3(100) + else: + self.collision_proxy_obj.translation = mn.Vector3(0) + + elif key == pressed.T: + if alt_pressed: + self.debug_draw_raycasts = not self.debug_draw_raycasts + print(f"Toggled self.debug_draw_raycasts: {self.debug_draw_raycasts}") + elif shift_pressed: + self.sample_seed -= 1 + else: + self.sample_seed += 1 + + event.accepted = True + return + # load URDF + fixed_base = alt_pressed + urdf_file_path = "" + if shift_pressed and self.cached_urdf: + urdf_file_path = self.cached_urdf + else: + urdf_file_path = input("Load URDF: provide a URDF filepath:").strip() + + if not urdf_file_path: + logger.warn("Load URDF: no input provided. Aborting.") + elif not urdf_file_path.endswith((".URDF", ".urdf")): + logger.warn("Load URDF: input is not a URDF. Aborting.") + elif os.path.exists(urdf_file_path): + self.cached_urdf = urdf_file_path + aom = self.sim.get_articulated_object_manager() + ao = aom.add_articulated_object_from_urdf( + urdf_file_path, fixed_base, 1.0, 1.0, True + ) + ao.translation = ( + self.default_agent.scene_node.transformation.transform_point( + [0.0, 1.0, -1.5] + ) + ) + else: + logger.warn("Load URDF: input file not found. Aborting.") + + elif key == pressed.M: + self.cycle_mouse_mode() + logger.info(f"Command: mouse mode set to {self.mouse_interaction}") + + elif key == pressed.V: + self.invert_gravity() + logger.info("Command: gravity inverted") + + elif key == pressed.N: + # (default) - toggle navmesh visualization + # NOTE: (+ALT) - re-sample the agent position on the NavMesh + # NOTE: (+SHIFT) - re-compute the NavMesh + if alt_pressed: + logger.info("Command: resample agent state from navmesh") + if self.sim.pathfinder.is_loaded: + new_agent_state = habitat_sim.AgentState() + new_agent_state.position = ( + self.sim.pathfinder.get_random_navigable_point() + ) + new_agent_state.rotation = quat_from_angle_axis( + self.sim.random.uniform_float(0, 2.0 * np.pi), + np.array([0, 1, 0]), + ) + self.default_agent.set_state(new_agent_state) + else: + logger.warning( + "NavMesh is not initialized. Cannot sample new agent state." + ) + elif shift_pressed: + logger.info("Command: recompute navmesh") + self.navmesh_config_and_recompute() + else: + if self.sim.pathfinder.is_loaded: + self.sim.navmesh_visualization = not self.sim.navmesh_visualization + logger.info("Command: toggle navmesh") + else: + logger.warn("Warning: recompute navmesh first") + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = True + event.accepted = True + self.redraw() + + def key_release_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key release. When a key is released, if it + is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the key will + be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = False + event.accepted = True + self.redraw() + + def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: + """ + Handles `Application.MouseMoveEvent`. When in LOOK mode, enables the left + mouse button to steer the agent's facing direction. When in GRAB mode, + continues to update the grabber's object position with our agents position. + """ + + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(event.position)) + self.mouse_cast_results = self.sim.cast_ray(ray=ray) + + button = Application.MouseMoveEvent.Buttons + # if interactive mode -> LOOK MODE + if event.buttons == button.LEFT and self.mouse_interaction == MouseMode.LOOK: + agent = self.sim.agents[self.agent_id] + delta = self.get_mouse_position(event.relative_position) / 2 + action = habitat_sim.agent.ObjectControls() + act_spec = habitat_sim.agent.ActuationSpec + + # left/right on agent scene node + action(agent.scene_node, "turn_right", act_spec(delta.x)) + + # up/down on cameras' scene nodes + action = habitat_sim.agent.ObjectControls() + sensors = list(self.default_agent.scene_node.subtree_sensors.values()) + [action(s.object, "look_down", act_spec(delta.y), False) for s in sensors] + + # if interactive mode is TRUE -> GRAB MODE + elif self.mouse_interaction == MouseMode.GRAB and self.mouse_grabber: + # update location of grabbed object + self.update_grab_position(self.get_mouse_position(event.position)) + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def construct_cylinder_object( + self, cyl_radius: float = 0.04, cyl_height: float = 0.15 + ): + constructed_cyl_temp_name = "scaled_cyl_template" + otm = self.sim.metadata_mediator.object_template_manager + cyl_temp_handle = otm.get_synth_template_handles("cylinder")[0] + cyl_temp = otm.get_template_by_handle(cyl_temp_handle) + cyl_temp.scale = mn.Vector3(cyl_radius, cyl_height / 2.0, cyl_radius) + otm.register_template(cyl_temp, constructed_cyl_temp_name) + return constructed_cyl_temp_name + + def mouse_press_event(self, event: Application.MouseEvent) -> None: + """ + Handles `Application.MouseEvent`. When in GRAB mode, click on + objects to drag their position. (right-click for fixed constraints) + """ + button = Application.MouseEvent.Button + physics_enabled = self.sim.get_physics_simulation_library() + + # if interactive mode is True -> GRAB MODE + if self.mouse_interaction == MouseMode.GRAB and physics_enabled: + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(event.position)) + raycast_results = self.sim.cast_ray(ray=ray) + + if raycast_results.has_hits(): + hit_object, ao_link = -1, -1 + hit_info = raycast_results.hits[0] + + if hit_info.object_id >= 0: + # we hit an non-staged collision object + ro_mngr = self.sim.get_rigid_object_manager() + ao_mngr = self.sim.get_articulated_object_manager() + ao = ao_mngr.get_object_by_id(hit_info.object_id) + ro = ro_mngr.get_object_by_id(hit_info.object_id) + + if ro: + # if grabbed an object + hit_object = hit_info.object_id + object_pivot = ro.transformation.inverted().transform_point( + hit_info.point + ) + object_frame = ro.rotation.inverted() + elif ao: + # if grabbed the base link + hit_object = hit_info.object_id + object_pivot = ao.transformation.inverted().transform_point( + hit_info.point + ) + object_frame = ao.rotation.inverted() + else: + for ao_handle in ao_mngr.get_objects_by_handle_substring(): + ao = ao_mngr.get_object_by_handle(ao_handle) + link_to_obj_ids = ao.link_object_ids + + if hit_info.object_id in link_to_obj_ids: + # if we got a link + ao_link = link_to_obj_ids[hit_info.object_id] + object_pivot = ( + ao.get_link_scene_node(ao_link) + .transformation.inverted() + .transform_point(hit_info.point) + ) + object_frame = ao.get_link_scene_node( + ao_link + ).rotation.inverted() + hit_object = ao.object_id + break + # done checking for AO + + if hit_object >= 0: + node = self.default_agent.scene_node + constraint_settings = physics.RigidConstraintSettings() + + constraint_settings.object_id_a = hit_object + constraint_settings.link_id_a = ao_link + constraint_settings.pivot_a = object_pivot + constraint_settings.frame_a = ( + object_frame.to_matrix() @ node.rotation.to_matrix() + ) + constraint_settings.frame_b = node.rotation.to_matrix() + constraint_settings.pivot_b = hit_info.point + + # by default use a point 2 point constraint + if event.button == button.RIGHT: + constraint_settings.constraint_type = ( + physics.RigidConstraintType.Fixed + ) + + grip_depth = ( + hit_info.point - render_camera.node.absolute_translation + ).length() + + self.mouse_grabber = MouseGrabber( + constraint_settings, + grip_depth, + self.sim, + ) + else: + logger.warn("Oops, couldn't find the hit object. That's odd.") + # end if didn't hit the scene + # end has raycast hit + # end has physics enabled + elif ( + self.mouse_interaction == MouseMode.LOOK + and physics_enabled + and self.mouse_cast_results is not None + and self.mouse_cast_results.has_hits() + and event.button == button.RIGHT + ): + constructed_cyl_obj_handle = self.construct_cylinder_object2() + # try to place an object + if ( + mn.math.dot( + self.mouse_cast_results.hits[0].normal.normalized(), + mn.Vector3(0, 1, 0), + ) + > 0.5 + ): + rom = self.sim.get_rigid_object_manager() + cyl_test_obj = rom.add_object_by_template_handle( + constructed_cyl_obj_handle + ) + assert cyl_test_obj is not None + cyl_test_obj.translation = self.mouse_cast_results.hits[ + 0 + ].point + mn.Vector3(0, 0.04, 0) + success = snap_down( + self.sim, + cyl_test_obj, + support_obj_ids=[-1, self.collision_proxy_obj.object_id], + ) + print(success) + if not success: + rom.remove_object_by_handle(cyl_test_obj.handle) + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def mouse_scroll_event(self, event: Application.MouseScrollEvent) -> None: + """ + Handles `Application.MouseScrollEvent`. When in LOOK mode, enables camera + zooming (fine-grained zoom using shift) When in GRAB mode, adjusts the depth + of the grabber's object. (larger depth change rate using shift) + """ + scroll_mod_val = ( + event.offset.y + if abs(event.offset.y) > abs(event.offset.x) + else event.offset.x + ) + if not scroll_mod_val: + return + + # use shift to scale action response + shift_pressed = bool(event.modifiers & Application.InputEvent.Modifier.SHIFT) + alt_pressed = bool(event.modifiers & Application.InputEvent.Modifier.ALT) + ctrl_pressed = bool(event.modifiers & Application.InputEvent.Modifier.CTRL) + + # if interactive mode is False -> LOOK MODE + if self.mouse_interaction == MouseMode.LOOK: + # use shift for fine-grained zooming + mod_val = 1.01 if shift_pressed else 1.1 + mod = mod_val if scroll_mod_val > 0 else 1.0 / mod_val + cam = self.render_camera + cam.zoom(mod) + self.redraw() + + elif self.mouse_interaction == MouseMode.GRAB and self.mouse_grabber: + # adjust the depth + mod_val = 0.1 if shift_pressed else 0.01 + scroll_delta = scroll_mod_val * mod_val + if alt_pressed or ctrl_pressed: + # rotate the object's local constraint frame + agent_t = self.default_agent.scene_node.transformation_matrix() + # ALT - yaw + rotation_axis = agent_t.transform_vector(mn.Vector3(0, 1, 0)) + if alt_pressed and ctrl_pressed: + # ALT+CTRL - roll + rotation_axis = agent_t.transform_vector(mn.Vector3(0, 0, -1)) + elif ctrl_pressed: + # CTRL - pitch + rotation_axis = agent_t.transform_vector(mn.Vector3(1, 0, 0)) + self.mouse_grabber.rotate_local_frame_by_global_angle_axis( + rotation_axis, mn.Rad(scroll_delta) + ) + else: + # update location of grabbed object + self.mouse_grabber.grip_depth += scroll_delta + self.update_grab_position(self.get_mouse_position(event.position)) + self.redraw() + event.accepted = True + + def mouse_release_event(self, event: Application.MouseEvent) -> None: + """ + Release any existing constraints. + """ + del self.mouse_grabber + self.mouse_grabber = None + event.accepted = True + + def update_grab_position(self, point: mn.Vector2i) -> None: + """ + Accepts a point derived from a mouse click event and updates the + transform of the mouse grabber. + """ + # check mouse grabber + if not self.mouse_grabber: + return + + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(point) + + rotation: mn.Matrix3x3 = self.default_agent.scene_node.rotation.to_matrix() + translation: mn.Vector3 = ( + render_camera.node.absolute_translation + + ray.direction * self.mouse_grabber.grip_depth + ) + self.mouse_grabber.update_transform(mn.Matrix4.from_(rotation, translation)) + + def get_mouse_position(self, mouse_event_position: mn.Vector2i) -> mn.Vector2i: + """ + This function will get a screen-space mouse position appropriately + scaled based on framebuffer size and window size. Generally these would be + the same value, but on certain HiDPI displays (Retina displays) they may be + different. + """ + scaling = mn.Vector2i(self.framebuffer_size) / mn.Vector2i(self.window_size) + return mouse_event_position * scaling + + def cycle_mouse_mode(self) -> None: + """ + This method defines how to cycle through the mouse mode. + """ + if self.mouse_interaction == MouseMode.LOOK: + self.mouse_interaction = MouseMode.GRAB + elif self.mouse_interaction == MouseMode.GRAB: + self.mouse_interaction = MouseMode.LOOK + + def navmesh_config_and_recompute(self) -> None: + """ + This method is setup to be overridden in for setting config accessibility + in inherited classes. + """ + self.navmesh_settings = habitat_sim.NavMeshSettings() + self.navmesh_settings.set_defaults() + self.navmesh_settings.agent_height = self.cfg.agents[self.agent_id].height + self.navmesh_settings.agent_radius = self.cfg.agents[self.agent_id].radius + + self.sim.recompute_navmesh( + self.sim.pathfinder, + self.navmesh_settings, + include_static_objects=True, + ) + + def exit_event(self, event: Application.ExitEvent): + """ + Overrides exit_event to properly close the Simulator before exiting the + application. + """ + for i in range(self.num_env): + self.tiled_sims[i].close(destroy=True) + event.accepted = True + exit(0) + + def draw_text(self, sensor_spec): + self.shader.bind_vector_texture(self.glyph_cache.texture) + self.shader.transformation_projection_matrix = self.window_text_transform + self.shader.color = [1.0, 1.0, 1.0] + + sensor_type_string = str(sensor_spec.sensor_type.name) + sensor_subtype_string = str(sensor_spec.sensor_subtype.name) + if self.mouse_interaction == MouseMode.LOOK: + mouse_mode_string = "LOOK" + elif self.mouse_interaction == MouseMode.GRAB: + mouse_mode_string = "GRAB" + self.window_text.render( + f""" +{self.fps} FPS +Sensor Type: {sensor_type_string} +Sensor Subtype: {sensor_subtype_string} +Mouse Interaction Mode: {mouse_mode_string} + """ + ) + self.shader.draw(self.window_text.mesh) + + def print_help_text(self) -> None: + """ + Print the Key Command help text. + """ + logger.info( + """ +===================================================== +Welcome to the Habitat-sim Python Viewer application! +===================================================== +Mouse Functions ('m' to toggle mode): +---------------- +In LOOK mode (default): + LEFT: + Click and drag to rotate the agent and look up/down. + WHEEL: + Modify orthographic camera zoom/perspective camera FOV (+SHIFT for fine grained control) + +In GRAB mode (with 'enable-physics'): + LEFT: + Click and drag to pickup and move an object with a point-to-point constraint (e.g. ball joint). + RIGHT: + Click and drag to pickup and move an object with a fixed frame constraint. + WHEEL (with picked object): + default - Pull gripped object closer or push it away. + (+ALT) rotate object fixed constraint frame (yaw) + (+CTRL) rotate object fixed constraint frame (pitch) + (+ALT+CTRL) rotate object fixed constraint frame (roll) + (+SHIFT) amplify scroll magnitude + + +Key Commands: +------------- + esc: Exit the application. + 'h': Display this help message. + 'm': Cycle mouse interaction modes. + + Agent Controls: + 'wasd': Move the agent's body forward/backward and left/right. + 'zx': Move the agent's body up/down. + arrow keys: Turn the agent's body left/right and camera look up/down. + + Utilities: + 'r': Reset the simulator with the most recently loaded scene. + 'n': Show/hide NavMesh wireframe. + (+SHIFT) Recompute NavMesh with default settings. + (+ALT) Re-sample the agent(camera)'s position and orientation from the NavMesh. + ',': Render a Bullet collision shape debug wireframe overlay (white=active, green=sleeping, blue=wants sleeping, red=can't sleep). + 'c': Run a discrete collision detection pass and render a debug wireframe overlay showing active contact points and normals (yellow=fixed length normals, red=collision distances). + (+SHIFT) Toggle the contact point debug render overlay on/off. + + Object Interactions: + SPACE: Toggle physics simulation on/off. + '.': Take a single simulation step if not simulating continuously. + 'v': (physics) Invert gravity. + 't': Load URDF from filepath + (+SHIFT) quick re-load the previously specified URDF + (+ALT) load the URDF with fixed base +===================================================== +""" + ) + + +class MouseMode(Enum): + LOOK = 0 + GRAB = 1 + MOTION = 2 + + +class MouseGrabber: + """ + Create a MouseGrabber from RigidConstraintSettings to manipulate objects. + """ + + def __init__( + self, + settings: physics.RigidConstraintSettings, + grip_depth: float, + sim: habitat_sim.simulator.Simulator, + ) -> None: + self.settings = settings + self.simulator = sim + + # defines distance of the grip point from the camera for pivot updates + self.grip_depth = grip_depth + self.constraint_id = sim.create_rigid_constraint(settings) + + def __del__(self): + self.remove_constraint() + + def remove_constraint(self) -> None: + """ + Remove a rigid constraint by id. + """ + self.simulator.remove_rigid_constraint(self.constraint_id) + + def updatePivot(self, pos: mn.Vector3) -> None: + self.settings.pivot_b = pos + self.simulator.update_rigid_constraint(self.constraint_id, self.settings) + + def update_frame(self, frame: mn.Matrix3x3) -> None: + self.settings.frame_b = frame + self.simulator.update_rigid_constraint(self.constraint_id, self.settings) + + def update_transform(self, transform: mn.Matrix4) -> None: + self.settings.frame_b = transform.rotation() + self.settings.pivot_b = transform.translation + self.simulator.update_rigid_constraint(self.constraint_id, self.settings) + + def rotate_local_frame_by_global_angle_axis( + self, axis: mn.Vector3, angle: mn.Rad + ) -> None: + """rotate the object's local constraint frame with a global angle axis input.""" + object_transform = mn.Matrix4() + rom = self.simulator.get_rigid_object_manager() + aom = self.simulator.get_articulated_object_manager() + if rom.get_library_has_id(self.settings.object_id_a): + object_transform = rom.get_object_by_id( + self.settings.object_id_a + ).transformation + else: + # must be an ao + object_transform = ( + aom.get_object_by_id(self.settings.object_id_a) + .get_link_scene_node(self.settings.link_id_a) + .transformation + ) + local_axis = object_transform.inverted().transform_vector(axis) + R = mn.Matrix4.rotation(angle, local_axis.normalized()) + self.settings.frame_a = R.rotation().__matmul__(self.settings.frame_a) + self.simulator.update_rigid_constraint(self.constraint_id, self.settings) + + +class Timer: + """ + Timer class used to keep track of time between buffer swaps + and guide the display frame rate. + """ + + start_time = 0.0 + prev_frame_time = 0.0 + prev_frame_duration = 0.0 + running = False + + @staticmethod + def start() -> None: + """ + Starts timer and resets previous frame time to the start time. + """ + Timer.running = True + Timer.start_time = time.time() + Timer.prev_frame_time = Timer.start_time + Timer.prev_frame_duration = 0.0 + + @staticmethod + def stop() -> None: + """ + Stops timer and erases any previous time data, resetting the timer. + """ + Timer.running = False + Timer.start_time = 0.0 + Timer.prev_frame_time = 0.0 + Timer.prev_frame_duration = 0.0 + + @staticmethod + def next_frame() -> None: + """ + Records previous frame duration and updates the previous frame timestamp + to the current time. If the timer is not currently running, perform nothing. + """ + if not Timer.running: + return + Timer.prev_frame_duration = time.time() - Timer.prev_frame_time + Timer.prev_frame_time = time.time() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + + # optional arguments + parser.add_argument( + "--scene", + default="./data/test_assets/scenes/simple_room.glb", + type=str, + help='scene/stage file to load (default: "./data/test_assets/scenes/simple_room.glb")', + ) + parser.add_argument( + "--dataset", + default="./data/objects/ycb/ycb.scene_dataset_config.json", + type=str, + metavar="DATASET", + help='dataset configuration file to use (default: "./data/objects/ycb/ycb.scene_dataset_config.json")', + ) + parser.add_argument( + "--disable-physics", + action="store_true", + help="disable physics simulation (default: False)", + ) + parser.add_argument( + "--reorient-object", + action="store_true", + help="reorient the object based on the values in the config file.", + ) + parser.add_argument( + "--stage-requires-lighting", + action="store_true", + help="Override configured lighting to use synthetic lighting for the stage.", + ) + parser.add_argument( + "--enable-batch-renderer", + action="store_true", + help="Enable batch rendering mode. The number of concurrent environments is specified with the num-environments parameter.", + ) + parser.add_argument( + "--num-environments", + default=1, + type=int, + help="Number of concurrent environments to batch render. Note that only the first environment simulates physics and can be controlled.", + ) + parser.add_argument( + "--composite-files", + type=str, + nargs="*", + help="Composite files that the batch renderer will use in-place of simulation assets to improve memory usage and performance. If none is specified, the original scene files will be loaded from disk.", + ) + parser.add_argument( + "--width", + default=800, + type=int, + help="Horizontal resolution of the window.", + ) + parser.add_argument( + "--height", + default=600, + type=int, + help="Vertical resolution of the window.", + ) + + args = parser.parse_args() + + if args.num_environments < 1: + parser.error("num-environments must be a positive non-zero integer.") + if args.width < 1: + parser.error("width must be a positive non-zero integer.") + if args.height < 1: + parser.error("height must be a positive non-zero integer.") + + # Setting up sim_settings + sim_settings: Dict[str, Any] = default_sim_settings + # sim_settings["scene"] = args.scene + sim_settings["scene"] = "NONE" + sim_settings["scene_dataset_config_file"] = args.dataset + sim_settings["enable_physics"] = not args.disable_physics + sim_settings["stage_requires_lighting"] = args.stage_requires_lighting + sim_settings["enable_batch_renderer"] = args.enable_batch_renderer + sim_settings["num_environments"] = args.num_environments + sim_settings["composite_files"] = args.composite_files + sim_settings["window_width"] = args.width + sim_settings["window_height"] = args.height + sim_settings["clear_color"] = mn.Color4.magenta() + + obj_name = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" + + # check against default + if args.scene != "./data/test_assets/scenes/simple_room.glb": + obj_name = args.scene + + # load JSON once instead of repeating + mm = habitat_sim.metadata.MetadataMediator() + mm.active_dataset = sim_settings["scene_dataset_config_file"] + + cpo = csa.CollisionProxyOptimizer(sim_settings) + obj_temp_handle = mm.object_template_manager.get_file_template_handles(obj_name)[0] + cpo.setup_obj_gt(obj_temp_handle) + cpo.compute_proxy_metrics(obj_temp_handle) + # setup globals for debug drawing + test_points = cpo.gt_data[obj_temp_handle]["test_points"] + pr_raycast_results = cpo.gt_data[obj_temp_handle]["raycasts"]["pr0"] + gt_raycast_results = cpo.gt_data[obj_temp_handle]["raycasts"]["gt"] + + # start the application + HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/examples/viewer.py b/examples/viewer.py index e1f5ee0195..1a29e16faf 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -17,26 +17,15 @@ import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle import magnum as mn import numpy as np -from habitat.sims.habitat_simulator.sim_utilities import snap_down from magnum import shaders, text from magnum.platform.glfw import Application import habitat_sim from habitat_sim import ReplayRenderer, ReplayRendererConfiguration, physics from habitat_sim.logging import LoggingContext, logger -from habitat_sim.utils.common import d3_40_colors_rgb, quat_from_angle_axis +from habitat_sim.utils.common import quat_from_angle_axis from habitat_sim.utils.settings import default_sim_settings, make_cfg -# add tools directory so I can import things to try them in the viewer -sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../tools")) -print(sys.path) -import collision_shape_automation as csa - -gt_raycast_results = None -pr_raycast_results = None -obj_temp_handle = None -test_points = None - class HabitatSimInteractiveViewer(Application): # the maximum number of chars displayable in the app window @@ -179,13 +168,12 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # toggle physics simulation on/off self.simulating = False - self.sample_seed = 0 - self.collision_proxy_obj = None - self.mouse_cast_results = None - self.debug_draw_raycasts = True - - self.debug_draw_receptacles = True - self.object_receptacles = [] + self.receptacles = None + self.display_receptacles = False + self.col_proxy_objs = None + self.col_proxies_visible = True + self.original_objs_visible = True + self.proxy_obj_postfix = "_collision_stand-in" # toggle a single simulation step at the next opportunity if not # simulating continuously. @@ -200,21 +188,43 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.reconfigure_sim() self.debug_semantic_colors = {} - if self.sim.pathfinder.is_loaded: - self.sim.pathfinder = habitat_sim.nav.PathFinder() - # compute NavMesh if not already loaded by the scene. - # if ( - # not self.sim.pathfinder.is_loaded - # and self.cfg.sim_cfg.scene_id.lower() != "none" - # ): - # self.navmesh_config_and_recompute() + if ( + not self.sim.pathfinder.is_loaded + and self.cfg.sim_cfg.scene_id.lower() != "none" + ): + self.navmesh_config_and_recompute() self.time_since_last_simulation = 0.0 LoggingContext.reinitialize_from_env() logger.setLevel("INFO") self.print_help_text() + def add_col_proxy_object( + self, obj_instance: habitat_sim.physics.ManagedRigidObject + ) -> habitat_sim.physics.ManagedRigidObject: + """ + Add a collision object visualization proxy to the scene overlapping with the given object. + Return the new proxy object. + """ + # replace the object with a collision_object + obj_temp_handle = obj_instance.creation_attributes.handle + otm = self.sim.get_object_template_manager() + object_template = otm.get_template_by_handle(obj_temp_handle) + object_template.scale = obj_instance.scale + np.ones(3) * 0.01 + object_template.render_asset_handle = object_template.collision_asset_handle + reg_id = otm.register_template( + object_template, + object_template.handle + self.proxy_obj_postfix, + ) + ro_mngr = self.sim.get_rigid_object_manager() + new_obj = ro_mngr.add_object_by_template_id(reg_id) + new_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + new_obj.translation = obj_instance.translation + new_obj.rotation = obj_instance.rotation + self.sim.set_object_bb_draw(True, new_obj.object_id) + return new_obj + def draw_contact_debug(self, debug_line_render: Any): """ This method is called to render a debug line overlay displaying active contact points and normals. @@ -312,43 +322,10 @@ def debug_draw(self): self.sim.get_rigid_object_manager().add_object_by_template_handle( obj_temp_handle ) - ) - self.collision_proxy_obj.motion_type = ( - habitat_sim.physics.MotionType.KINEMATIC - ) - - csa.debug_draw_raycast_results( - self.sim, gt_raycast_results, pr_raycast_results, seed=self.sample_seed - ) - - # draw test points - for side in test_points: - for p in side: - self.sim.get_debug_line_render().draw_circle( - translation=p, - radius=0.005, - color=mn.Color4.magenta(), - ) - - if self.debug_draw_receptacles and self.collision_proxy_obj is not None: - # parse any receptacles defined for the object - if len(self.object_receptacles) == 0: - source_template_file = ( - self.collision_proxy_obj.creation_attributes.file_directory - ) - user_attr = self.collision_proxy_obj.user_attributes - self.object_receptacles = ( - hab_receptacle.parse_receptacles_from_user_config( - user_attr, - parent_object_handle=self.collision_proxy_obj.handle, - parent_template_directory=source_template_file, - ) - ) - # draw any receptacles for the object - for rix, receptacle in enumerate(self.object_receptacles): - c = d3_40_colors_rgb[rix] - rec_color = mn.Vector3(c[0], c[1], c[2]) / 256.0 - receptacle.debug_draw(self.sim, color=mn.Color4.from_xyz(rec_color)) + # only display receptacles within 4 meters + if mn.math.dot((c_to_r).normalized(), c_forward) > 0.75: + # only display receptacles centered in view + receptacle.debug_draw(self.sim) def draw_event( self, @@ -471,49 +448,6 @@ def reconfigure_sim(self) -> None: self.cfg.sim_cfg.override_scene_light_defaults = True self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY - # create custom stage from object - self.cfg.metadata_mediator = habitat_sim.metadata.MetadataMediator() - self.cfg.metadata_mediator.active_dataset = self.sim_settings[ - "scene_dataset_config_file" - ] - if args.reorient_object: - obj_handle = ( - self.cfg.metadata_mediator.object_template_manager.get_template_handles( - args.scene - )[0] - ) - fp_models_metadata_file = ( - "/home/alexclegg/Documents/dev/fphab/fpModels_metadata.csv" - ) - obj_orientations = csa.parse_object_orientations_from_metadata_csv( - fp_models_metadata_file - ) - csa.correct_object_orientations( - [obj_handle], obj_orientations, self.cfg.metadata_mediator - ) - - otm = self.cfg.metadata_mediator.object_template_manager - obj_template = otm.get_template_by_handle(obj_temp_handle) - obj_template.compute_COM_from_shape = False - obj_template.com = mn.Vector3(0) - otm.register_template(obj_template) - stm = self.cfg.metadata_mediator.stage_template_manager - stage_template_name = "obj_as_stage_template" - new_stage_template = stm.create_new_template(handle=stage_template_name) - new_stage_template.render_asset_handle = obj_template.render_asset_handle - new_stage_template.orient_up = obj_template.orient_up - new_stage_template.orient_front = obj_template.orient_front - - # margin must be 0 for snapping to work on overlapped gt/proxy - new_stage_template.margin = 0.0 - stm.register_template( - template=new_stage_template, specified_handle=stage_template_name - ) - self.cfg.sim_cfg.scene_id = stage_template_name - # visualize the object as its collision shape - obj_template.render_asset_handle = obj_template.collision_asset_handle - otm.register_template(obj_template) - if self.sim is None: self.tiled_sims = [] for _i in range(self.num_env): @@ -703,25 +637,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.contact_debug_draw = True # TODO: add a nice log message with concise contact pair naming. - elif key == pressed.O: - # move the object in/out of the frame - if self.collision_proxy_obj is not None: - if self.collision_proxy_obj.translation == mn.Vector3(0): - self.collision_proxy_obj.translation = mn.Vector3(100) - else: - self.collision_proxy_obj.translation = mn.Vector3(0) - elif key == pressed.T: - if alt_pressed: - self.debug_draw_raycasts = not self.debug_draw_raycasts - print(f"Toggled self.debug_draw_raycasts: {self.debug_draw_raycasts}") - elif shift_pressed: - self.sample_seed -= 1 - else: - self.sample_seed += 1 - - event.accepted = True - return # load URDF fixed_base = alt_pressed urdf_file_path = "" @@ -829,11 +745,6 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: mouse button to steer the agent's facing direction. When in GRAB mode, continues to update the grabber's object position with our agents position. """ - - render_camera = self.render_camera.render_camera - ray = render_camera.unproject(self.get_mouse_position(event.position)) - self.mouse_cast_results = self.sim.cast_ray(ray=ray) - button = Application.MouseMoveEvent.Buttons # if interactive mode -> LOOK MODE if event.buttons == button.LEFT and self.mouse_interaction == MouseMode.LOOK: @@ -859,17 +770,6 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: self.redraw() event.accepted = True - def construct_cylinder_object( - self, cyl_radius: float = 0.04, cyl_height: float = 0.15 - ): - constructed_cyl_temp_name = "scaled_cyl_template" - otm = self.sim.metadata_mediator.object_template_manager - cyl_temp_handle = otm.get_synth_template_handles("cylinder")[0] - cyl_temp = otm.get_template_by_handle(cyl_temp_handle) - cyl_temp.scale = mn.Vector3(cyl_radius, cyl_height / 2.0, cyl_radius) - otm.register_template(cyl_temp, constructed_cyl_temp_name) - return constructed_cyl_temp_name - def mouse_press_event(self, event: Application.MouseEvent) -> None: """ Handles `Application.MouseEvent`. When in GRAB mode, click on @@ -962,38 +862,6 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: # end if didn't hit the scene # end has raycast hit # end has physics enabled - elif ( - self.mouse_interaction == MouseMode.LOOK - and physics_enabled - and self.mouse_cast_results is not None - and self.mouse_cast_results.has_hits() - and event.button == button.RIGHT - ): - constructed_cyl_obj_handle = self.construct_cylinder_object2() - # try to place an object - if ( - mn.math.dot( - self.mouse_cast_results.hits[0].normal.normalized(), - mn.Vector3(0, 1, 0), - ) - > 0.5 - ): - rom = self.sim.get_rigid_object_manager() - cyl_test_obj = rom.add_object_by_template_handle( - constructed_cyl_obj_handle - ) - assert cyl_test_obj is not None - cyl_test_obj.translation = self.mouse_cast_results.hits[ - 0 - ].point + mn.Vector3(0, 0.04, 0) - success = snap_down( - self.sim, - cyl_test_obj, - support_obj_ids=[-1, self.collision_proxy_obj.object_id], - ) - print(success) - if not success: - rom.remove_object_by_handle(cyl_test_obj.handle) self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() @@ -1410,8 +1278,7 @@ def next_frame() -> None: # Setting up sim_settings sim_settings: Dict[str, Any] = default_sim_settings - # sim_settings["scene"] = args.scene - sim_settings["scene"] = "NONE" + sim_settings["scene"] = args.scene sim_settings["scene_dataset_config_file"] = args.dataset sim_settings["enable_physics"] = not args.disable_physics sim_settings["use_default_lighting"] = args.use_default_lighting @@ -1424,24 +1291,5 @@ def next_frame() -> None: sim_settings["enable_hbao"] = args.hbao sim_settings["clear_color"] = mn.Color4.magenta() - obj_name = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" - - # check against default - if args.scene != "./data/test_assets/scenes/simple_room.glb": - obj_name = args.scene - - # load JSON once instead of repeating - mm = habitat_sim.metadata.MetadataMediator() - mm.active_dataset = sim_settings["scene_dataset_config_file"] - - cpo = csa.CollisionProxyOptimizer(sim_settings) - obj_temp_handle = mm.object_template_manager.get_file_template_handles(obj_name)[0] - cpo.setup_obj_gt(obj_temp_handle) - cpo.compute_proxy_metrics(obj_temp_handle) - # setup globals for debug drawing - test_points = cpo.gt_data[obj_temp_handle]["test_points"] - pr_raycast_results = cpo.gt_data[obj_temp_handle]["raycasts"]["pr0"] - gt_raycast_results = cpo.gt_data[obj_temp_handle]["raycasts"]["gt"] - # start the application HabitatSimInteractiveViewer(sim_settings).exec() From 16d4edf23645abec1ca9e06febfcc1421871771c Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 14 Apr 2023 17:04:21 -0700 Subject: [PATCH 029/153] add mouse object info in right click for scene viewer --- examples/viewer.py | 43 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index 1a29e16faf..09a1f7fa7d 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -168,13 +168,20 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # toggle physics simulation on/off self.simulating = False + + # receptacle visualization self.receptacles = None self.display_receptacles = False + + # collision proxy visualization self.col_proxy_objs = None self.col_proxies_visible = True self.original_objs_visible = True self.proxy_obj_postfix = "_collision_stand-in" + # mouse raycast visualization + self.mouse_cast_results = None + # toggle a single simulation step at the next opportunity if not # simulating continuously. self.simulate_single_step = False @@ -323,10 +330,20 @@ def debug_draw(self): obj_temp_handle ) # only display receptacles within 4 meters - if mn.math.dot((c_to_r).normalized(), c_forward) > 0.75: + if mn.math.dot((c_to_r).normalized(), c_forward) > 0.7: # only display receptacles centered in view receptacle.debug_draw(self.sim) + # mouse raycast circle + white = mn.Color4(mn.Vector3(1.0), 1.0) + if self.mouse_cast_results is not None and self.mouse_cast_results.has_hits(): + self.sim.get_debug_line_render().draw_circle( + translation=self.mouse_cast_results.hits[0].point, + radius=0.005, + color=white, + normal=self.mouse_cast_results.hits[0].normal, + ) + def draw_event( self, simulation_call: Optional[Callable] = None, @@ -745,6 +762,11 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: mouse button to steer the agent's facing direction. When in GRAB mode, continues to update the grabber's object position with our agents position. """ + + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(event.position)) + self.mouse_cast_results = self.sim.cast_ray(ray=ray) + button = Application.MouseMoveEvent.Buttons # if interactive mode -> LOOK MODE if event.buttons == button.LEFT and self.mouse_interaction == MouseMode.LOOK: @@ -862,6 +884,25 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: # end if didn't hit the scene # end has raycast hit # end has physics enabled + elif ( + self.mouse_interaction == MouseMode.LOOK + and physics_enabled + and self.mouse_cast_results is not None + and self.mouse_cast_results.has_hits() + and event.button == button.RIGHT + ): + hit_id = self.mouse_cast_results.hits[0].object_id + rom = self.sim.get_rigid_object_manager() + # right click in look mode to print object information + if hit_id == -1: + print("This is the stage.") + elif rom.get_library_has_id(hit_id): + ro = rom.get_object_by_id(hit_id) + print(f"Rigid Object: {ro.handle}") + if self.receptacles is not None: + for rec in self.receptacles: + if rec.parent_object_handle == ro.handle: + print(f" - Receptacle: {rec.name}") self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() From 528988083d86fd883a7478afafa228e7643cc689 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Sat, 15 Apr 2023 09:33:11 -0700 Subject: [PATCH 030/153] add CPO use in viewer to discplay rec metrics and filter. Add terminal attr setting. --- examples/obj_viewer.py | 2 +- examples/viewer.py | 228 ++++++++++++++++++++++++++-- tools/collision_shape_automation.py | 10 +- 3 files changed, 223 insertions(+), 17 deletions(-) diff --git a/examples/obj_viewer.py b/examples/obj_viewer.py index 561b853944..75433cc5df 100644 --- a/examples/obj_viewer.py +++ b/examples/obj_viewer.py @@ -1371,7 +1371,7 @@ def next_frame() -> None: mm = habitat_sim.metadata.MetadataMediator() mm.active_dataset = sim_settings["scene_dataset_config_file"] - cpo = csa.CollisionProxyOptimizer(sim_settings) + cpo = csa.CollisionProxyOptimizer(sim_settings, None, mm) obj_temp_handle = mm.object_template_manager.get_file_template_handles(obj_name)[0] cpo.setup_obj_gt(obj_temp_handle) cpo.compute_proxy_metrics(obj_temp_handle) diff --git a/examples/viewer.py b/examples/viewer.py index 09a1f7fa7d..d71e8c8104 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -26,6 +26,58 @@ from habitat_sim.utils.common import quat_from_angle_axis from habitat_sim.utils.settings import default_sim_settings, make_cfg +# add tools directory so I can import things to try them in the viewer +sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../tools")) +print(sys.path) +import collision_shape_automation as csa + +# CollisionProxyOptimizer initialized before the application +_cpo: Optional[csa.CollisionProxyOptimizer] = None +_cpo_threads = [] + + +def _cpo_initialized(): + global _cpo + global _cpo_threads + if _cpo is None: + return False + return all(not thread.is_alive() for thread in _cpo_threads) + + +class RecColorMode(Enum): + """ + Defines the coloring mode for receptacle debug drawing. + """ + + DEFAULT = 0 # all magenta + GT_ACCESS = 1 # red to green + GT_STABILITY = 2 + PR_ACCESS = 3 + PR_STABILITY = 4 + + +class ColorLERP: + """ + xyz lerp between two colors. + """ + + def __init__(self, c0: mn.Color4, c1: mn.Color4): + self.c0 = c0.to_xyz() + self.c1 = c1.to_xyz() + self.delta = self.c1 - self.c0 + + def at(self, t: float) -> mn.Color4: + """ + Compute the LERP at time t [0,1]. + """ + assert t >= 0 and t <= 1, "Extrapolation not recommended in color space." + t_color_xyz = self.c0 + self.delta * t + return mn.Color4.from_xyz(t_color_xyz) + + +# red to green lerp for heatmaps +rg_lerp = ColorLERP(mn.Color4.red(), mn.Color4.green()) + class HabitatSimInteractiveViewer(Application): # the maximum number of chars displayable in the app window @@ -45,7 +97,11 @@ class HabitatSimInteractiveViewer(Application): # CPU and GPU usage info DISPLAY_FONT_SIZE = 16.0 - def __init__(self, sim_settings: Dict[str, Any]) -> None: + def __init__( + self, + sim_settings: Dict[str, Any], + mm: Optional[habitat_sim.metadata.MetadataMediator] = None, + ) -> None: self.sim_settings: Dict[str:Any] = sim_settings self.enable_batch_renderer: bool = self.sim_settings["enable_batch_renderer"] @@ -172,6 +228,14 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # receptacle visualization self.receptacles = None self.display_receptacles = False + global _cpo + self._cpo = _cpo + self.cpo_initialized = False + self.filter_rec_by_access = False + self.rec_access_filter_threshold = 0.12 # empirically chosen + self.rec_color_mode = RecColorMode.DEFAULT + # map receptacle to parent objects + self.rec_to_poh: Dict[hab_receptacle.Receptacle, str] = {} # collision proxy visualization self.col_proxy_objs = None @@ -207,6 +271,41 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: logger.setLevel("INFO") self.print_help_text() + def modify_param_from_term(self): + """ + Prompts the user to enter an attribute name and new value. + Attempts to fulfill the user's request. + """ + # first get an attribute + user_attr = input("++++++++++++\nProvide an attribute to edit: ") + if not hasattr(self, user_attr): + print(f" The '{user_attr}' attribute does not exist.") + return + + # then get a value + user_val = input(f"Now provide a value for '{user_attr}': ") + cur_attr_val = getattr(self, user_attr) + if cur_attr_val is not None: + try: + # try type conversion + new_val = type(cur_attr_val)(user_val) + + # special handling for bool because all strings become True with cast + if isinstance(cur_attr_val, bool): + if user_val.lower() == "false": + new_val = False + elif user_val.lower() == "true": + new_val = True + + setattr(self, user_attr, new_val) + print( + f"attr '{user_attr}' set to '{getattr(self, user_attr)}' (type={type(new_val)})." + ) + except Exception: + print(f"Failed to cast '{user_val}' to {type(cur_attr_val)}.") + else: + print("That attribute is unset, so I don't know the type.") + def add_col_proxy_object( self, obj_instance: habitat_sim.physics.ManagedRigidObject ) -> habitat_sim.physics.ManagedRigidObject: @@ -331,8 +430,54 @@ def debug_draw(self): ) # only display receptacles within 4 meters if mn.math.dot((c_to_r).normalized(), c_forward) > 0.7: - # only display receptacles centered in view - receptacle.debug_draw(self.sim) + # handle coloring + rec_color = None + if ( + self.cpo_initialized + and self.rec_color_mode != RecColorMode.DEFAULT + ): + if self.rec_color_mode == RecColorMode.GT_STABILITY: + rec_color = rg_lerp.at( + self._cpo.gt_data[self.rec_to_poh[receptacle]][ + "receptacles" + ][receptacle.name]["shape_id_results"]["gt"][ + "stability_results" + ][ + "success_ratio" + ] + ) + elif self.rec_color_mode == RecColorMode.GT_ACCESS: + rec_color = rg_lerp.at( + self._cpo.gt_data[self.rec_to_poh[receptacle]][ + "receptacles" + ][receptacle.name]["shape_id_results"]["gt"][ + "access_results" + ][ + "receptacle_access_score" + ] + ) + elif self.rec_color_mode == RecColorMode.PR_STABILITY: + rec_color = rg_lerp.at( + self._cpo.gt_data[self.rec_to_poh[receptacle]][ + "receptacles" + ][receptacle.name]["shape_id_results"]["pr0"][ + "stability_results" + ][ + "success_ratio" + ] + ) + elif self.rec_color_mode == RecColorMode.PR_ACCESS: + rec_color = rg_lerp.at( + self._cpo.gt_data[self.rec_to_poh[receptacle]][ + "receptacles" + ][receptacle.name]["shape_id_results"]["pr0"][ + "access_results" + ][ + "receptacle_access_score" + ] + ) + + receptacle.debug_draw(self.sim, color=rec_color) # mouse raycast circle white = mn.Color4(mn.Vector3(1.0), 1.0) @@ -354,6 +499,10 @@ def draw_event( Calls continuously to re-render frames and swap the two frame buffers at a fixed rate. """ + # until cpo initialization is finished, keep checking + if not self.cpo_initialized: + self.cpo_initialized = _cpo_initialized() + agent_acts_per_sec = self.fps mn.gl.default_framebuffer.clear( @@ -444,7 +593,9 @@ def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: ) return agent_config - def reconfigure_sim(self) -> None: + def reconfigure_sim( + self, mm: Optional[habitat_sim.metadata.MetadataMediator] = None + ) -> None: """ Utilizes the current `self.sim_settings` to configure and set up a new `habitat_sim.Simulator`, and then either starts a simulation instance, or replaces @@ -452,6 +603,7 @@ def reconfigure_sim(self) -> None: """ # configure our sim_settings but then set the agent to our default self.cfg = make_cfg(self.sim_settings) + self.cfg.metadata_mediator = mm self.agent_id: int = self.sim_settings["default_agent"] self.cfg.agents[self.agent_id] = self.default_agent_config() @@ -655,13 +807,15 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # TODO: add a nice log message with concise contact pair naming. elif key == pressed.T: + self.modify_param_from_term() + # load URDF - fixed_base = alt_pressed - urdf_file_path = "" - if shift_pressed and self.cached_urdf: - urdf_file_path = self.cached_urdf - else: - urdf_file_path = input("Load URDF: provide a URDF filepath:").strip() + # fixed_base = alt_pressed + # urdf_file_path = "" + # if shift_pressed and self.cached_urdf: + # urdf_file_path = self.cached_urdf + # else: + # urdf_file_path = input("Load URDF: provide a URDF filepath:").strip() if not urdf_file_path: logger.warn("Load URDF: no input provided. Aborting.") @@ -1239,6 +1393,45 @@ def next_frame() -> None: Timer.prev_frame_time = time.time() +def init_cpo_for_scene(sim_settings, mm: habitat_sim.metadata.MetadataMediator): + """ + Initialize and run th CPO for all objects in the scene. + """ + global _cpo + global _cpo_threads + + _cpo = csa.CollisionProxyOptimizer(sim_settings, None, mm) + + # get object handles from a specific scene + objects_in_scene = csa.get_objects_in_scene( + dataset_path=sim_settings["scene_dataset_config_file"], + scene_handle=sim_settings["scene"], + mm=_cpo.mm, + ) + # get a subset with receptacles defined + objects_in_scene = [ + objects_in_scene[i] + for i in range(len(objects_in_scene)) + if csa.object_has_receptacles(objects_in_scene[i], mm.object_template_manager) + ] + + def run_cpo_for_obj(obj_handle): + _cpo.setup_obj_gt(obj_handle) + _cpo.compute_receptacle_stability(obj_handle, use_gt=True) + _cpo.compute_receptacle_stability(obj_handle) + _cpo.compute_receptacle_access_metrics(obj_handle, use_gt=True) + _cpo.compute_receptacle_access_metrics(obj_handle, use_gt=False) + + # run CPO initialization multi-threaded to unblock viewer initialization and use + import threading + + threads = [] + for obj_handle in objects_in_scene: + threads.append(threading.Thread(target=run_cpo_for_obj, args=(obj_handle,))) + for thread in threads: + thread.start() + + if __name__ == "__main__": import argparse @@ -1258,6 +1451,11 @@ def next_frame() -> None: metavar="DATASET", help='dataset configuration file to use (default: "default")', ) + parser.add_argument( + "--init-cpo", + action="store_true", + help="Initialize and run the CPO for the current scene.", + ) parser.add_argument( "--disable-physics", action="store_true", @@ -1332,5 +1530,13 @@ def next_frame() -> None: sim_settings["enable_hbao"] = args.hbao sim_settings["clear_color"] = mn.Color4.magenta() + mm = habitat_sim.metadata.MetadataMediator() + mm.active_dataset = sim_settings["scene_dataset_config_file"] + + # initialize the CPO. + # this will be done in parallel to viewer setup via multithreading + if args.init_cpo: + init_cpo_for_scene(sim_settings, mm) + # start the application - HabitatSimInteractiveViewer(sim_settings).exec() + HabitatSimInteractiveViewer(sim_settings, mm).exec() diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 9931168b25..39ff3ee3bf 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -2123,12 +2123,12 @@ def main(): # ---------------------------------------------------- # run the pipeline for a set of scenes with separate output files for each - scenes_of_interest = [] + scenes_of_interest = ["102816036"] # get all scenes from the mm - scenes_of_interest = [ - handle.split(".scene_instance.json")[0].split("/")[-1] - for handle in cpo.mm.get_scene_handles() - ] + # scenes_of_interest = [ + # handle.split(".scene_instance.json")[0].split("/")[-1] + # for handle in cpo.mm.get_scene_handles() + # ] for scene_of_interest in scenes_of_interest: cpo.init_caches() From d1c2ae73b31e35ec3c90e92d0612071b5c73c0b4 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Sat, 15 Apr 2023 10:36:27 -0700 Subject: [PATCH 031/153] add caching and disply of per-point access and stability metrics --- examples/viewer.py | 37 +++++++++++------------------ tools/collision_shape_automation.py | 10 ++++++++ 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index d71e8c8104..31c8dd71ed 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -237,6 +237,9 @@ def __init__( # map receptacle to parent objects self.rec_to_poh: Dict[hab_receptacle.Receptacle, str] = {} + # display stability samples for selected object w/ receptacle + self.display_selected_stability_samples = True + # collision proxy visualization self.col_proxy_objs = None self.col_proxies_visible = True @@ -245,6 +248,8 @@ def __init__( # mouse raycast visualization self.mouse_cast_results = None + # last clicked or None for stage + self.selected_object = None # toggle a single simulation step at the next opportunity if not # simulating continuously. @@ -438,43 +443,27 @@ def debug_draw(self): ): if self.rec_color_mode == RecColorMode.GT_STABILITY: rec_color = rg_lerp.at( - self._cpo.gt_data[self.rec_to_poh[receptacle]][ - "receptacles" - ][receptacle.name]["shape_id_results"]["gt"][ + rec_dat["shape_id_results"]["gt"][ "stability_results" - ][ - "success_ratio" - ] + ]["success_ratio"] ) elif self.rec_color_mode == RecColorMode.GT_ACCESS: rec_color = rg_lerp.at( - self._cpo.gt_data[self.rec_to_poh[receptacle]][ - "receptacles" - ][receptacle.name]["shape_id_results"]["gt"][ - "access_results" - ][ + rec_dat["shape_id_results"]["gt"]["access_results"][ "receptacle_access_score" ] ) elif self.rec_color_mode == RecColorMode.PR_STABILITY: rec_color = rg_lerp.at( - self._cpo.gt_data[self.rec_to_poh[receptacle]][ - "receptacles" - ][receptacle.name]["shape_id_results"]["pr0"][ + rec_dat["shape_id_results"]["pr0"][ "stability_results" - ][ - "success_ratio" - ] + ]["success_ratio"] ) elif self.rec_color_mode == RecColorMode.PR_ACCESS: rec_color = rg_lerp.at( - self._cpo.gt_data[self.rec_to_poh[receptacle]][ - "receptacles" - ][receptacle.name]["shape_id_results"]["pr0"][ + rec_dat["shape_id_results"]["pr0"][ "access_results" - ][ - "receptacle_access_score" - ] + ]["receptacle_access_score"] ) receptacle.debug_draw(self.sim, color=rec_color) @@ -1045,6 +1034,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: and self.mouse_cast_results.has_hits() and event.button == button.RIGHT ): + self.selected_object = None hit_id = self.mouse_cast_results.hits[0].object_id rom = self.sim.get_rigid_object_manager() # right click in look mode to print object information @@ -1052,6 +1042,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: print("This is the stage.") elif rom.get_library_has_id(hit_id): ro = rom.get_object_by_id(hit_id) + self.selected_object = ro print(f"Rigid Object: {ro.handle}") if self.receptacles is not None: for rec in self.receptacles: diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 39ff3ee3bf..d434c81bf5 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1046,6 +1046,9 @@ def compute_receptacle_access_metrics( obj_rec_data[receptacle_name]["shape_id_results"][shape_id][ "access_results" ] = { + "receptacle_point_access_scores": receptacle_point_access_scores[ + receptacle_name + ], "sample_point_ray_results": sample_point_ray_results, "receptacle_access_score": receptacle_access_score, "receptacle_access_rate": receptacle_access_rate, @@ -1212,6 +1215,7 @@ def compute_receptacle_stability( failed_snap = 0 failed_by_distance = 0 failed_unstable = 0 + point_stabilities = [] for sample_point in sample_points: cyl_test_obj.translation = sample_point cyl_test_obj.rotation = mn.Quaternion.identity_init() @@ -1229,6 +1233,7 @@ def compute_receptacle_stability( ) if expected_height_error > accepted_height_error: failed_by_distance += 1 + point_stabilities.append(False) continue # physical stability analysis @@ -1262,8 +1267,12 @@ def compute_receptacle_stability( # NOTE: we assume that if the object has not moved past the threshold in 'max_sim_time', then it must be stabel enough if not object_is_stable: failed_unstable += 1 + point_stabilities.append(False) + else: + point_stabilities.append(True) else: failed_snap += 1 + point_stabilities.append(False) successful_points = ( len(sample_points) @@ -1295,6 +1304,7 @@ def compute_receptacle_stability( "failed_by_distance": failed_by_distance, "failed_unstable": failed_unstable, "total": len(sample_points), + "point_stabilities": point_stabilities, } def setup_shape_test_results_cache(self, obj_handle: str, shape_id: str) -> None: From 55b737f98ca3f670b9c3a18ff76c2754e16f8bdb Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 17 Apr 2023 17:23:55 -0700 Subject: [PATCH 032/153] minor fixes and cleanup for obj_viewer --- examples/obj_viewer.py | 49 +++++++++++++++++++++++++++++------------- 1 file changed, 34 insertions(+), 15 deletions(-) diff --git a/examples/obj_viewer.py b/examples/obj_viewer.py index 75433cc5df..0502cae27f 100644 --- a/examples/obj_viewer.py +++ b/examples/obj_viewer.py @@ -56,8 +56,13 @@ class HabitatSimInteractiveViewer(Application): # CPU and GPU usage info DISPLAY_FONT_SIZE = 16.0 - def __init__(self, sim_settings: Dict[str, Any]) -> None: + def __init__( + self, + sim_settings: Dict[str, Any], + mm: Optional[habitat_sim.metadata.MetadataMediator] = None, + ) -> None: self.sim_settings: Dict[str:Any] = sim_settings + self.mm = mm self.enable_batch_renderer: bool = self.sim_settings["enable_batch_renderer"] self.num_env: int = ( @@ -435,6 +440,7 @@ def reconfigure_sim(self) -> None: """ # configure our sim_settings but then set the agent to our default self.cfg = make_cfg(self.sim_settings) + self.cfg.metadata_mediator = mm self.agent_id: int = self.sim_settings["default_agent"] self.cfg.agents[self.agent_id] = self.default_agent_config() @@ -449,14 +455,15 @@ def reconfigure_sim(self) -> None: self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY # create custom stage from object - self.cfg.metadata_mediator = habitat_sim.metadata.MetadataMediator() + if self.cfg.metadata_mediator is None: + self.cfg.metadata_mediator = habitat_sim.metadata.MetadataMediator() self.cfg.metadata_mediator.active_dataset = self.sim_settings[ "scene_dataset_config_file" ] if args.reorient_object: obj_handle = ( self.cfg.metadata_mediator.object_template_manager.get_template_handles( - args.scene + args.target_object )[0] ) fp_models_metadata_file = ( @@ -489,6 +496,10 @@ def reconfigure_sim(self) -> None: self.cfg.sim_cfg.scene_id = stage_template_name # visualize the object as its collision shape obj_template.render_asset_handle = obj_template.collision_asset_handle + print(f"obj_template.render_asset_handle = {obj_template.render_asset_handle}") + print( + f"obj_template.collision_asset_handle = {obj_template.collision_asset_handle}" + ) otm.register_template(obj_template) if self.sim is None: @@ -923,7 +934,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: and self.mouse_cast_results.has_hits() and event.button == button.RIGHT ): - constructed_cyl_obj_handle = self.construct_cylinder_object2() + constructed_cyl_obj_handle = self.construct_cylinder_object() # try to place an object if ( mn.math.dot( @@ -1281,10 +1292,15 @@ def next_frame() -> None: # optional arguments parser.add_argument( - "--scene", - default="./data/test_assets/scenes/simple_room.glb", + "--target-object", + type=str, + help="object file to load.", + ) + parser.add_argument( + "--col-obj", + default=None, type=str, - help='scene/stage file to load (default: "./data/test_assets/scenes/simple_room.glb")', + help="Collision object file to use.", ) parser.add_argument( "--dataset", @@ -1349,7 +1365,7 @@ def next_frame() -> None: # Setting up sim_settings sim_settings: Dict[str, Any] = default_sim_settings - # sim_settings["scene"] = args.scene + # sim_settings["scene"] = args.target_object sim_settings["scene"] = "NONE" sim_settings["scene_dataset_config_file"] = args.dataset sim_settings["enable_physics"] = not args.disable_physics @@ -1361,18 +1377,21 @@ def next_frame() -> None: sim_settings["window_height"] = args.height sim_settings["clear_color"] = mn.Color4.magenta() - obj_name = "d1d1e0cdaba797ee70882e63f66055675c3f1e7f" - - # check against default - if args.scene != "./data/test_assets/scenes/simple_room.glb": - obj_name = args.scene + obj_name = args.target_object # load JSON once instead of repeating mm = habitat_sim.metadata.MetadataMediator() mm.active_dataset = sim_settings["scene_dataset_config_file"] - cpo = csa.CollisionProxyOptimizer(sim_settings, None, mm) obj_temp_handle = mm.object_template_manager.get_file_template_handles(obj_name)[0] + + # set a custom collision asset + if args.col_obj is not None: + obj_temp = mm.object_template_manager.get_template_by_handle(obj_temp_handle) + obj_temp.collision_asset_handle = args.col_obj + mm.object_template_manager.register_template(obj_temp) + + cpo = csa.CollisionProxyOptimizer(sim_settings, None, mm) cpo.setup_obj_gt(obj_temp_handle) cpo.compute_proxy_metrics(obj_temp_handle) # setup globals for debug drawing @@ -1381,4 +1400,4 @@ def next_frame() -> None: gt_raycast_results = cpo.gt_data[obj_temp_handle]["raycasts"]["gt"] # start the application - HabitatSimInteractiveViewer(sim_settings).exec() + HabitatSimInteractiveViewer(sim_settings, mm).exec() From bdfddf20c5a344d3d2d62750657c4c45c4e92249 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 17 Apr 2023 17:25:50 -0700 Subject: [PATCH 033/153] changes supporting collision shape optimization at scene level --- tools/collision_shape_automation.py | 219 +++++++++++++++++++++++++--- 1 file changed, 201 insertions(+), 18 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index d434c81bf5..d488596b66 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -4,12 +4,20 @@ import argparse import csv +import ctypes import math import os import random +import sys import time from typing import Any, Dict, List, Optional, Tuple +# not adding this causes some failures in mesh import +flags = sys.getdlopenflags() +sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) + +from collections import defaultdict + # imports from Habitat-lab # NOTE: requires PR 1108 branch: rearrange-gen-improvements (https://github.com/facebookresearch/habitat-lab/pull/1108) import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle @@ -1639,12 +1647,13 @@ def grid_search_vhacd_params(self, obj_template_handle: str): # "pca": (0,1), #pca seems worse, no speed improvement # "mode": (0,1), #tetrahedral mode seems worse "alpha": (0.05, 0.1), + # "alpha": (0.05,), "beta": (0.05, 0.1), - "plane_downsampling": [2], - "convex_hull_downsampling": [2], - "max_num_vertices_per_ch": (16, 32), - # "max_convex_hulls": (8,16,32,64), - "max_convex_hulls": (16, 32, 64), + "plane_downsampling": [1], + "convex_hull_downsampling": [1], + # "max_num_vertices_per_ch": (16, 32), + "max_convex_hulls": (64, 128), + # "max_convex_hulls": (500,), "resolution": [200000], } @@ -1682,20 +1691,25 @@ def grid_search_vhacd_params(self, obj_template_handle: str): shape_id = self.get_proxy_shape_id(obj_template_handle) if "vhacd_settings" not in self.gt_data[obj_template_handle]: self.gt_data[obj_template_handle]["vhacd_settings"] = {} - self.gt_data[obj_template_handle]["vhacd_settings"][ - shape_id - ] = setting_string + self.gt_data[obj_template_handle]["vhacd_settings"][shape_id] = ( + vhacd_params, + setting_string, + ) # compute shape level metrics: self.compute_proxy_metrics(obj_template_handle) - self.compute_grid_collision_times(obj_template_handle, subdivisions=1) - self.run_physics_settle_test(obj_template_handle) - self.run_physics_sphere_shake_test(obj_template_handle) + # self.compute_grid_collision_times(obj_template_handle, subdivisions=1) + # self.run_physics_settle_test(obj_template_handle) + # self.run_physics_sphere_shake_test(obj_template_handle) # compute receptacle metrics if self.compute_receptacle_useability_metrics: - self.compute_receptacle_access_metrics(obj_handle=obj_template_handle) - self.compute_receptacle_stability(obj_handle=obj_template_handle) + self.compute_receptacle_access_metrics( + obj_handle=obj_template_handle, use_gt=False + ) + self.compute_receptacle_stability( + obj_handle=obj_template_handle, use_gt=False + ) vhacd_iteration_times[shape_id] = time.time() - vhacd_iteration_time print(f"Total VHACD time = {time.time()-vhacd_start_time}") @@ -1703,7 +1717,141 @@ def grid_search_vhacd_params(self, obj_template_handle: str): for shape_id, settings in self.gt_data[obj_template_handle][ "vhacd_settings" ].items(): - print(f" {shape_id} - {settings} - {vhacd_iteration_times[shape_id]}") + print( + f" {shape_id} - {settings[1]} - {vhacd_iteration_times[shape_id]}" + ) + + def optimize_object_col_shape_vhacd(self, obj_h: str, col_shape_dir: str): + """ + Run VHACD optimization for a specific object. + Identify the optimal collision shape and save the result as the new default. + + :return: Tuple(best_shape_id, best_shape_score, original_shape_score) if best_shape_id == "pr0", then optimization didn't change anything. + """ + otm = self.mm.object_template_manager + obj_temp = otm.get_template_by_handle(obj_h) + cur_col_shape_path = os.path.abspath(obj_temp.collision_asset_handle) + self.setup_obj_gt(obj_h) + self.compute_proxy_metrics(obj_h) + self.compute_receptacle_stability(obj_h, use_gt=True) + self.compute_receptacle_stability(obj_h) + self.compute_receptacle_access_metrics(obj_h, use_gt=True) + self.compute_receptacle_access_metrics(obj_h, use_gt=False) + self.grid_search_vhacd_params(obj_h) + self.compute_gt_errors(obj_h) + + # time to select the best version + best_shape_id = "pr0" + pr0_shape_score = ( + 1.0 + - self.gt_data[obj_h]["shape_test_results"]["pr0"][ + "normalized_raycast_error" + ] + ) + best_shape_score = pr0_shape_score + shape_scores = {} + access_scores = {"gt": {}, "pr0": {}} + stab_ratios = {"gt": {}, "pr0": {}} + rel_access_scores = defaultdict(dict) + rel_stab_scores = defaultdict(dict) + for shape_id in self.gt_data[obj_h]["vhacd_settings"].keys(): + # compare shape metric + if ( + self.gt_data[obj_h]["shape_test_results"][shape_id][ + "normalized_raycast_error" + ] + > self.gt_data[obj_h]["shape_test_results"]["pr0"][ + "normalized_raycast_error" + ] + ): + # worse metric performance than the default, skip it. + continue + + shape_score = ( + 1.0 + - self.gt_data[obj_h]["shape_test_results"][shape_id][ + "normalized_raycast_error" + ] + ) + + # compare rec metrics + for rec_name, rec_data in self.gt_data[obj_h]["receptacles"].items(): + for orig_shape_id in access_scores.keys(): + if rec_name not in access_scores[orig_shape_id]: + access_scores[orig_shape_id][rec_name] = rec_data[ + "shape_id_results" + ][orig_shape_id]["access_results"]["receptacle_access_score"] + if rec_name not in stab_ratios[orig_shape_id]: + stab_ratios[orig_shape_id][rec_name] = rec_data[ + "shape_id_results" + ][orig_shape_id]["stability_results"]["success_ratio"] + + sh_rec_dat = rec_data["shape_id_results"][shape_id] + rel_access_scores[shape_id][rec_name] = ( + access_scores["gt"][rec_name] + - sh_rec_dat["access_results"]["receptacle_access_score"] + ) + rel_stab_scores[shape_id][rec_name] = ( + stab_ratios["gt"][rec_name] + - sh_rec_dat["stability_results"]["success_ratio"] + ) + + if ( + access_scores["gt"][rec_name] < 0.15 + or stab_ratios["gt"][rec_name] < 0.5 + ): + "this receptacle is not good anyway, so skip it" + continue + + if rec_name not in rel_access_scores["pr0"]: + rel_access_scores["pr0"][rec_name] = ( + access_scores["gt"][rec_name] - access_scores["pr0"][rec_name] + ) + pr0_shape_score += rel_access_scores["pr0"][rec_name] + if best_shape_id == "pr0": + best_shape_score = pr0_shape_score + if rec_name not in rel_stab_scores["pr0"]: + rel_stab_scores["pr0"][rec_name] = ( + stab_ratios["gt"][rec_name] - stab_ratios["pr0"][rec_name] + ) + pr0_shape_score += rel_stab_scores["pr0"][rec_name] + if best_shape_id == "pr0": + best_shape_score = pr0_shape_score + + # TODO: weight these up? + shape_score += rel_stab_scores[shape_id][rec_name] + shape_score += rel_access_scores[shape_id][rec_name] + + shape_scores[shape_id] = shape_score + if shape_score > best_shape_score: + best_shape_id = shape_id + best_shape_score = shape_score + + print(self.gt_data[obj_h]["vhacd_settings"]) + print(shape_scores) + + if best_shape_id != "pr0": + # re-save the best version + print( + f"Best shape_id = {best_shape_id} with shape score {best_shape_score} better than 'pr0' with shape score {pr0_shape_score}." + ) + self.compute_vhacd_col_shape( + obj_h, self.gt_data[obj_h]["vhacd_settings"][best_shape_id][0] + ) + # copy the collision asset into the correct directory + obj_name = obj_h.split(".object_config.json")[0].split("/")[-1] + col_shape_path = os.path.join(col_shape_dir, obj_name + ".obj") + os.system(f"obj2gltf -i {col_shape_path} -o {cur_col_shape_path}") + else: + print( + f"Best shape_id = {best_shape_id} with shape score {best_shape_score}." + ) + + best_shape_params = None + if best_shape_id != "pr0": + best_shape_params = self.gt_data[obj_h]["vhacd_settings"][best_shape_id] + self.clean_obj_gt(obj_h) + return (best_shape_id, best_shape_score, pr0_shape_score, best_shape_params) def compute_vhacd_col_shape( self, obj_template_handle: str, vhacd_params: habitat_sim.VHACDParameters @@ -1737,8 +1885,16 @@ def compute_vhacd_col_shape( vhacd_template = otm.get_template_by_handle(matching_vhacd_handles[0]) - obj_template.collision_asset_handle = vhacd_template.collision_asset_handle + # copy the file to glb + new_filename = vhacd_template.collision_asset_handle.split(".obj")[0] + ".glb" + command = ( + f"obj2gltf -i {vhacd_template.collision_asset_handle} -o {new_filename}" + ) + + os.system(command) + print(command) + obj_template.collision_asset_handle = new_filename otm.register_template(obj_template) def cache_global_results(self) -> None: @@ -2131,6 +2287,19 @@ def main(): # cpo.compute_and_save_results_for_objects(all_handles) # ---------------------------------------------------- + # ---------------------------------------------------- + # specific object handle + # 163cdd355d2d228880dd6ed6f3fc5b49f7ba538a + # 174ea8cf2a80b38b2ce4dbbda6de2fee33d3870b + # 5277bce5d54fba0ab5ee869ed623932b447457d4 + # obj_of_interest = "163cdd355d2d228880dd6ed6f3fc5b49f7ba538a" + # obj_of_interest = "174ea8cf2a80b38b2ce4dbbda6de2fee33d3870b" + # obj_h = otm.get_file_template_handles(obj_of_interest)[0] + # results = cpo.optimize_object_col_shape_vhacd(obj_h,col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/") + # print(f"opt_results = {results}") + # exit() + # ---------------------------------------------------- + # ---------------------------------------------------- # run the pipeline for a set of scenes with separate output files for each scenes_of_interest = ["102816036"] @@ -2174,9 +2343,23 @@ def main(): correct_object_orientations(all_handles, obj_orientations, cpo.mm) # ---------------------------------------------------- - cpo.compute_and_save_results_for_objects( - all_handles, output_filename=scene_of_interest + "_cpo_out" - ) + # run VHACD opt for all shapes + results = [] + for obj_h in all_handles: + results.append( + cpo.optimize_object_col_shape_vhacd( + obj_h, + col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/", + ) + ) + + print("Finished all objects in the scene!") + for oix, obj_h in enumerate(all_handles): + print(f" Object Handle = {obj_h}:") + print(f" results = {results[oix]}") + # cpo.compute_and_save_results_for_objects( + # all_handles, output_filename=scene_of_interest + "_cpo_out" + # ) if __name__ == "__main__": From dcd44aa19e07e0b77451a44f1b5501e0bbb6644c Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 17 Apr 2023 17:27:23 -0700 Subject: [PATCH 034/153] viewer.py changes supporting receptacle filtering and loading/saving filter metadata json. --- examples/viewer.py | 248 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 246 insertions(+), 2 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 31c8dd71ed..866641f12b 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -3,6 +3,7 @@ # LICENSE file in the root directory of this source tree. import ctypes +import json import math import os import string @@ -54,6 +55,7 @@ class RecColorMode(Enum): GT_STABILITY = 2 PR_ACCESS = 3 PR_STABILITY = 4 + FILTERING = 5 # colored by filter status (green=active, yellow=manually filtered, red=automatically filtered (access), magenta=automatically filtered (access), blue=automatically filtered (height)) class ColorLERP: @@ -231,11 +233,14 @@ def __init__( global _cpo self._cpo = _cpo self.cpo_initialized = False - self.filter_rec_by_access = False + self.show_filtered = True self.rec_access_filter_threshold = 0.12 # empirically chosen self.rec_color_mode = RecColorMode.DEFAULT # map receptacle to parent objects self.rec_to_poh: Dict[hab_receptacle.Receptacle, str] = {} + # contains filtering metadata and classification of meshes filtered automatically and manually + self.rec_filter_data = None + self.rec_filter_path = self.sim_settings["rec_filter_file"] # display stability samples for selected object w/ receptacle self.display_selected_stability_samples = True @@ -250,6 +255,7 @@ def __init__( self.mouse_cast_results = None # last clicked or None for stage self.selected_object = None + self.selected_rec = None # toggle a single simulation step at the next opportunity if not # simulating continuously. @@ -311,6 +317,157 @@ def modify_param_from_term(self): else: print("That attribute is unset, so I don't know the type.") + def get_rec_instance_name(self, receptacle: hab_receptacle.Receptacle) -> str: + """ + Gets a unique string name for the Receptacle instance. + Multiple Receptacles can share a name (e.g. if the object has multiple instances in the scene). + The unique name is constructed as '|'. + """ + rec_unique_name = receptacle.parent_object_handle + "|" + receptacle.name + return rec_unique_name + + def get_closest_tri_receptacle( + self, pos: mn.Vector3, max_dist: float = 3.5 + ) -> Optional[hab_receptacle.TriangleMeshReceptacle]: + """ + Return the closest receptacle to the given position or None. + + :param pos: The point to compare with receptacle verts. + :param max_dist: The maximum allowable distance to the receptacle to count. + + :return: None if failed or closest receptacle. + """ + if self.receptacles is None or not self.display_receptacles: + return None + closest_rec = None + closest_rec_dist = max_dist + for receptacle in self.receptacles: + g_trans = receptacle.get_global_transform(self.sim) + # transform the query point once instead of all verts + local_point = g_trans.inverted().transform_point(pos) + if (g_trans.translation - pos).length() < max_dist: + # receptacles object transform should be close to the point + for vert in receptacle.mesh_data.attribute( + mn.trade.MeshAttribute.POSITION + ): + v_dist = (local_point - vert).length() + if v_dist < closest_rec_dist: + closest_rec_dist = v_dist + closest_rec = receptacle + return closest_rec + + def compute_rec_filter_state( + self, + access_threshold: float = 0.12, + stab_threshold: float = 0.5, + filter_shape: str = "pr0", + ) -> None: + """ + Check all receptacles against automated filters to fill the + + :param access_threshold: Access threshold for filtering. Roughly % of sample points with some raycast access. + :param stab_threshold: Stability threshold for filtering. Roughly % of sample points with stable object support. + :param filter_shape: Which shape metrics to use for filter. Choices typically "gt"(ground truth) or "pr0"(proxy shape). + """ + # load receptacles if not done + if self.receptacles is None: + self.load_receptacles() + assert ( + self._cpo is not None + ), "Must initialize the CPO before automatic filtering. Re-run with '--init-cpo'." + + # initialize if necessary + if self.rec_filter_data is None: + self.rec_filter_data = { + "active": [], + "manually_filtered": [], + "access_filtered": [], + "access_threshold": access_threshold, # set in filter procedure + "stability_filtered": [], + "stability threshold": stab_threshold, # set in filter procedure + # TODO: + "height_filtered": [], + "max_height": 0, + "min_height": 0, + } + + for rec in self.receptacles: + rec_unique_name = self.get_rec_instance_name(rec) + # respect already marked receptacles + if rec_unique_name not in self.rec_filter_data["manually_filtered"]: + rec_dat = self._cpo.gt_data[self.rec_to_poh[rec]]["receptacles"][ + rec.name + ] + rec_shape_data = rec_dat["shape_id_results"][filter_shape] + # filter by access + if ( + "access_results" in rec_shape_data + and rec_shape_data["access_results"]["receptacle_access_score"] + < access_threshold + ): + self.rec_filter_data["access_filtered"].append(rec_unique_name) + # filter by stability + elif ( + "stability_results" in rec_shape_data + and rec_shape_data["stability_results"]["success_ratio"] + < stab_threshold + ): + self.rec_filter_data["stability_filtered"].append(rec_unique_name) + # TODO: add more filters + # TODO: 1. filter by height relative to the floor + # TODO: 2. filter outdoor (raycast up) + # TODO: 3/4: filter by access/stability in scene context (relative to other objects) + # remaining receptacles are active + else: + self.rec_filter_data["active"].append(rec_unique_name) + + def export_filtered_recs(self, filepath: Optional[str] = None) -> None: + """ + Save a JSON with filtering metadata and filtered Receptacles for a scene. + + :param filepath: Defines the output filename for this JSON. If omitted, defaults to "./rec_filter_data.json". + """ + if filepath is None: + filepath = "rec_filter_data.json" + os.makedirs(os.path.dirname(filepath), exist_ok=True) + with open(filepath, "w") as f: + f.write(json.dumps(self.rec_filter_data, indent=2)) + + def load_filtered_recs(self, filepath: Optional[str] = None) -> None: + """ + Load a Receptacle filtering metadata JSON to visualize the state of the scene. + + :param filepath: Defines the input filename for this JSON. If omitted, defaults to "./rec_filter_data.json". + """ + if filepath is None: + filepath = "rec_filter_data.json" + if not os.path.exists(filepath): + print(f"Filtered rec metadata file {filepath} does not exist. Cannot load.") + return + with open(filepath, "r") as f: + self.rec_filter_data = json.load(f) + + # assert the format is correct + assert "active" in self.rec_filter_data + assert "manually_filtered" in self.rec_filter_data + assert "access_filtered" in self.rec_filter_data + assert "stability_filtered" in self.rec_filter_data + assert "height_filtered" in self.rec_filter_data + + def load_receptacles(self): + """ + Load all receptacle data and setup helper datastructures. + """ + self.receptacles = hab_receptacle.find_receptacles(self.sim) + for receptacle in self.receptacles: + if receptacle not in self.rec_to_poh: + po_handle = ( + self.sim.get_rigid_object_manager() + .get_object_by_handle(receptacle.parent_object_handle) + .creation_attributes.handle + ) + self.rec_to_poh[receptacle] = po_handle + def add_col_proxy_object( self, obj_instance: habitat_sim.physics.ManagedRigidObject ) -> habitat_sim.physics.ManagedRigidObject: @@ -437,7 +594,10 @@ def debug_draw(self): if mn.math.dot((c_to_r).normalized(), c_forward) > 0.7: # handle coloring rec_color = None - if ( + if self.selected_rec == receptacle: + # white + rec_color = mn.Color4.cyan() + elif ( self.cpo_initialized and self.rec_color_mode != RecColorMode.DEFAULT ): @@ -465,6 +625,29 @@ def debug_draw(self): "access_results" ]["receptacle_access_score"] ) + elif self.rec_color_mode == RecColorMode.FILTERING: + if rec_unique_name in self.rec_filter_data["active"]: + rec_color = mn.Color4.green() + elif ( + rec_unique_name + in self.rec_filter_data["manually_filtered"] + ): + rec_color = mn.Color4.yellow() + elif ( + rec_unique_name + in self.rec_filter_data["access_filtered"] + ): + rec_color = mn.Color4.red() + elif ( + rec_unique_name + in self.rec_filter_data["stability_filtered"] + ): + rec_color = mn.Color4.magenta() + elif ( + rec_unique_name + in self.rec_filter_data["height_filtered"] + ): + rec_color = mn.Color4.blue() receptacle.debug_draw(self.sim, color=rec_color) @@ -942,6 +1125,9 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: """ button = Application.MouseEvent.Button physics_enabled = self.sim.get_physics_simulation_library() + mod = Application.InputEvent.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) # if interactive mode is True -> GRAB MODE if self.mouse_interaction == MouseMode.GRAB and physics_enabled: @@ -1035,6 +1221,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: and event.button == button.RIGHT ): self.selected_object = None + self.selected_rec = None hit_id = self.mouse_cast_results.hits[0].object_id rom = self.sim.get_rigid_object_manager() # right click in look mode to print object information @@ -1048,6 +1235,57 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: for rec in self.receptacles: if rec.parent_object_handle == ro.handle: print(f" - Receptacle: {rec.name}") + if shift_pressed: + self.selected_rec = self.get_closest_tri_receptacle( + self.mouse_cast_results.hits[0].point + ) + if self.selected_rec is not None: + print(f"Selected Receptacle: {self.selected_rec.name}") + elif alt_pressed: + filtered_rec = self.get_closest_tri_receptacle( + self.mouse_cast_results.hits[0].point + ) + if filtered_rec is not None: + filtered_rec_name = self.get_rec_instance_name(filtered_rec) + print(f"Modified Receptacle Filter State: {filtered_rec_name}") + if ( + filtered_rec_name + in self.rec_filter_data["manually_filtered"] + ): + print(" remove from manual filter") + # this was manually filtered, remove it and try to make active + self.rec_filter_data["manually_filtered"].remove( + filtered_rec_name + ) + add_to_active = True + for other_out_set in [ + "access_filtered", + "stability_filtered", + "height_filtered", + ]: + if ( + filtered_rec_name + in self.rec_filter_data[other_out_set] + ): + print(f" is in {other_out_set}") + add_to_active = False + break + if add_to_active: + print(" is active") + self.rec_filter_data["active"].append(filtered_rec_name) + elif filtered_rec_name in self.rec_filter_data["active"]: + print(" remove from active, add manual filter") + # this was active, remove it and mark manually filtered + self.rec_filter_data["active"].remove(filtered_rec_name) + self.rec_filter_data["manually_filtered"].append( + filtered_rec_name + ) + else: + print(" add to manual filter, but has other filter") + # this is already filtered, but add it to manual filters + self.rec_filter_data["manually_filtered"].append( + filtered_rec_name + ) self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() @@ -1442,6 +1680,12 @@ def run_cpo_for_obj(obj_handle): metavar="DATASET", help='dataset configuration file to use (default: "default")', ) + parser.add_argument( + "--rec-filter-file", + default="./rec_filter_data.json", + type=str, + help='Receptacle filtering metadata (default: "./rec_filter_data.json")', + ) parser.add_argument( "--init-cpo", action="store_true", From e7e867c2138743081218c8e305c5cd2b71812c80 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 18 Apr 2023 18:12:55 -0700 Subject: [PATCH 035/153] COACD optimization, shape score improvement, viewer documentation. --- examples/viewer.py | 26 +- tools/collision_shape_automation.py | 379 +++++++++++++++++++--------- 2 files changed, 287 insertions(+), 118 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 866641f12b..d9815f77d3 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -459,6 +459,11 @@ def load_receptacles(self): Load all receptacle data and setup helper datastructures. """ self.receptacles = hab_receptacle.find_receptacles(self.sim) + self.receptacles = [ + rec + for rec in self.receptacles + if "collision_stand-in" not in rec.parent_object_handle + ] for receptacle in self.receptacles: if receptacle not in self.rec_to_poh: po_handle = ( @@ -481,6 +486,7 @@ def add_col_proxy_object( object_template = otm.get_template_by_handle(obj_temp_handle) object_template.scale = obj_instance.scale + np.ones(3) * 0.01 object_template.render_asset_handle = object_template.collision_asset_handle + object_template.is_collidable = False reg_id = otm.register_template( object_template, object_template.handle + self.proxy_obj_postfix, @@ -1462,7 +1468,10 @@ def print_help_text(self) -> None: Click and drag to rotate the agent and look up/down. WHEEL: Modify orthographic camera zoom/perspective camera FOV (+SHIFT for fine grained control) - + RIGHT: + Click an object to select the object. Prints object name and attached receptacle names. Selected object displays sample points when cpo is initialized. + (+SHIFT) select a receptacle. + (+ALT) add or remove a receptacle from the "manual filter set". In GRAB mode (with 'enable-physics'): LEFT: Click and drag to pickup and move an object with a point-to-point constraint (e.g. ball joint). @@ -1500,10 +1509,17 @@ def print_help_text(self) -> None: Object Interactions: SPACE: Toggle physics simulation on/off. '.': Take a single simulation step if not simulating continuously. - 'v': (physics) Invert gravity. - 't': Load URDF from filepath - (+SHIFT) quick re-load the previously specified URDF - (+ALT) load the URDF with fixed base + + Receptacle Evaluation Tool UI: + 'v': Load all Receptacles for the scene and toggle Receptacle visibility. + (+SHIFT) Iterate through receptacle color modes. + 'f': Toggle Receptacle view filtering. When on, only non-filtered Receptacles are visible. + (+SHIFT) Export current filter metadata to file. + (+ALT) Import filter metadata from file. + 'o': Toggle display of collision proxy shapes for the scene. + (+SHIFT) Toggle display of original render shapes (and Receptacles). + 't': CLI for modifying un-bound viewer parameters during runtime. + ===================================================== """ ) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index d488596b66..e9699351c1 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -12,11 +12,13 @@ import time from typing import Any, Dict, List, Optional, Tuple +import coacd +import trimesh + # not adding this causes some failures in mesh import flags = sys.getdlopenflags() sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) -from collections import defaultdict # imports from Habitat-lab # NOTE: requires PR 1108 branch: rearrange-gen-improvements (https://github.com/facebookresearch/habitat-lab/pull/1108) @@ -118,6 +120,28 @@ def get_scaled_hemisphere_vectors(scale: float): return [v * scale for v in icosphere_points_subdiv_3] +class COACDParams: + def __init__( + self, + ) -> None: + # Parameter tuning tricks from https://github.com/SarahWeiii/CoACD: + + # The default parameters are fast versions. If you care less about running time but more about the number of components, try to increase searching depth, searching node, and searching iteration for better cutting strategies. + self.threshold = 0.05 # adjust the threshold (0.01~1) to balance the level of detail and the number of decomposed components. A higher value gives coarser results, and a lower value gives finer-grained results. You can refer to Fig. 14 in our paper for more details. + self.max_convex_hull = -1 + self.preprocess = True # ensure input mesh is 2-manifold solid if you want to skip pre-process. Skipping manifold pre-processing can better preserve input details, but can crash or fail otherwise if input is not manifold. + self.preprocess_resolution = 30 # controls the quality of manifold preprocessing. A larger value can make the preprocessed mesh closer to the original mesh but also lead to more triangles and longer runtime. + self.mcts_nodes = 20 + self.mcts_iterations = 150 + self.mcts_max_depth = 3 + self.pca = False + self.merge = True + self.seed = 0 + + def __str__(self) -> str: + return f"COACDParams(threshold={self.threshold} | max_convex_hull={self.max_convex_hull} | preprocess={self.preprocess} | preprocess_resolution={self.preprocess_resolution} | mcts_nodes={self.mcts_nodes} | mcts_iterations={self.mcts_iterations} | mcts_max_depth={self.mcts_max_depth} | pca={self.pca} | merge={self.merge} | seed={self.seed})" + + def print_dict_structure(input_dict: Dict[Any, Any], whitespace: str = "") -> None: """ Quick structure investigation for dictionary. @@ -1623,6 +1647,163 @@ def compute_gt_errors(self, obj_handle: str) -> None: "normalized_raycast_error" ] = normalized_error + def get_obj_render_mesh_filepath(self, obj_template_handle: str): + """ + Return the filepath of the render mesh for an object. + """ + otm = self.mm.object_template_manager + obj_template = otm.get_template_by_handle(obj_template_handle) + assert obj_template is not None, "Object template is not registerd." + return os.path.abspath(obj_template.render_asset_handle) + + def permute_param_variations( + self, param_ranges: Dict[str, List[Any]] + ) -> List[List[Any]]: + """ + Generate a list of all permutations of the provided parameter ranges defined in a Dict. + """ + permutations = [[]] + + # permute variations + for attr, values in param_ranges.items(): + new_permutations = [] + for v in values: + for permutation in permutations: + extended_permutation = [(attr, v)] + for setting in permutation: + extended_permutation.append(setting) + new_permutations.append(extended_permutation) + permutations = new_permutations + print(f"Parameter permutations = {len(permutations)}") + for setting in permutations: + print(f" {setting}") + + return permutations + + def run_coacd_grid_search(self, obj_template_handle: str): + """ + Run grid search on relevant COACD params for an object. + """ + + # Parameter tuning tricks from https://github.com/SarahWeiii/CoACD in definition of COACDParams. + + param_ranges = { + "threshold": [0.04, 0.01], + } + + permutations = self.permute_param_variations(param_ranges) + + coacd_start_time = time.time() + coacd_iteration_times = {} + coacd_num_hulls = {} + # evaluate COACD settings + for setting in permutations: + coacd_param = COACDParams() + setting_string = "" + for attr, val in setting: + setattr(coacd_param, attr, val) + setting_string += f" '{attr}'={val}" + + coacd_iteration_time = time.time() + output_file, num_hulls = self.run_coacd( + obj_template_handle, coacd_param + ) + # setup the proxy + otm = self.mm.object_template_manager + obj_template = otm.get_template_by_handle(obj_template_handle) + obj_template.collision_asset_handle = output_file + otm.register_template(obj_template) + + self.increment_proxy_index(obj_template_handle) + + shape_id = self.get_proxy_shape_id(obj_template_handle) + if "coacd_settings" not in self.gt_data[obj_template_handle]: + self.gt_data[obj_template_handle]["coacd_settings"] = {} + self.gt_data[obj_template_handle]["coacd_settings"][shape_id] = ( + coacd_param, + setting_string, + ) + + self.compute_proxy_metrics(obj_template_handle) + # self.compute_grid_collision_times(obj_template_handle, subdivisions=1) + # self.run_physics_settle_test(obj_template_handle) + # self.run_physics_sphere_shake_test(obj_template_handle) + if self.compute_receptacle_useability_metrics: + self.compute_receptacle_access_metrics( + obj_handle=obj_template_handle, use_gt=False + ) + self.compute_receptacle_stability( + obj_handle=obj_template_handle, use_gt=False + ) + coacd_iteration_times[shape_id] = time.time() - coacd_iteration_time + coacd_num_hulls[shape_id] = num_hulls + + print(f"Total CAOCD time = {time.time()-coacd_start_time}") + print(" Iteration times = ") + for shape_id, settings in self.gt_data[obj_template_handle][ + "coacd_settings" + ].items(): + print( + f" {shape_id} - {settings[1]} - {coacd_iteration_times[shape_id]}" + ) + print(coacd_num_hulls) + + def run_coacd( + self, + obj_template_handle: str, + params: COACDParams, + output_file: Optional[str] = None, + ) -> str: + """ + Run COACD on an object given a set of parameters producing a file. + If output_file is not provided, defaults to "COACD_output/obj_name.glb" where obj_name is truncated handle (filename, no path or file ending). + """ + if output_file is None: + obj_name = obj_template_handle.split(".object_config.json")[0].split("/")[ + -1 + ] + output_file = "COACD_output/" + obj_name + ".glb" + os.makedirs(os.path.dirname(output_file), exist_ok=True) + input_filepath = self.get_obj_render_mesh_filepath(obj_template_handle) + # TODO: this seems dirty, maybe refactor: + tris = trimesh.load(input_filepath).triangles + verts = [] + indices = [] + v_counter = 0 + for tri in tris: + indices.append([v_counter, v_counter + 1, v_counter + 2]) + v_counter += 3 + for vert in tri: + verts.append(vert) + imesh = coacd.Mesh() + imesh.vertices = verts + imesh.indices = indices + parts = coacd.run_coacd( + imesh, + threshold=params.threshold, + max_convex_hull=params.max_convex_hull, + preprocess=params.preprocess, + preprocess_resolution=params.preprocess_resolution, + mcts_nodes=params.mcts_nodes, + mcts_iterations=params.mcts_iterations, + mcts_max_depth=params.mcts_max_depth, + pca=params.pca, + merge=params.merge, + seed=params.seed, + ) + mesh_parts = [ + trimesh.Trimesh(np.array(p.vertices), np.array(p.indices).reshape((-1, 3))) + for p in parts + ] + scene = trimesh.Scene() + + np.random.seed(0) + for p in mesh_parts: + p.visual.vertex_colors[:, :3] = (np.random.rand(3) * 255).astype(np.uint8) + scene.add_geometry(p) + scene.export(output_file) + return output_file, len(parts) + def grid_search_vhacd_params(self, obj_template_handle: str): """ For a specified set of search parameters, try all combinations by: @@ -1657,21 +1838,7 @@ def grid_search_vhacd_params(self, obj_template_handle: str): "resolution": [200000], } - permutations = [[]] - - # permute variations - for attr, values in param_ranges.items(): - new_permutations = [] - for v in values: - for permutation in permutations: - extended_permutation = [(attr, v)] - for setting in permutation: - extended_permutation.append(setting) - new_permutations.append(extended_permutation) - permutations = new_permutations - print(f"Parameter permutations = {len(permutations)}") - for setting in permutations: - print(f" {setting}") + permutations = self.permute_param_variations(param_ranges) vhacd_start_time = time.time() vhacd_iteration_times = {} @@ -1721,7 +1888,44 @@ def grid_search_vhacd_params(self, obj_template_handle: str): f" {shape_id} - {settings[1]} - {vhacd_iteration_times[shape_id]}" ) - def optimize_object_col_shape_vhacd(self, obj_h: str, col_shape_dir: str): + def compute_shape_score(self, obj_h: str, shape_id: str) -> float: + """ + Compute the shape score for the given object and shape_id. + Higher shape score is better performance on the metrics. + """ + shape_score = 0 + + # start with normalized error + normalized_error = self.gt_data[obj_h]["shape_test_results"][shape_id][ + "normalized_raycast_error" + ] + shape_score -= normalized_error + + # sum up scores for al receptacles + for _rec_name, rec_data in self.gt_data[obj_h]["receptacles"].items(): + sh_rec_dat = rec_data["shape_id_results"][shape_id] + gt_rec_dat = rec_data["shape_id_results"]["gt"] + gt_access = gt_rec_dat["access_results"]["receptacle_access_score"] + gt_stability = gt_rec_dat["stability_results"]["success_ratio"] + + # filter out generally bad receptacles from the score + if gt_access < 0.15 or gt_stability < 0.5: + "this receptacle is not good anyway, so skip it" + continue + + # penalize different acces than ground truth (more access than gt is also bad as it implies worse overall shape matching) + rel_access_score = abs( + gt_access - sh_rec_dat["access_results"]["receptacle_access_score"] + ) + shape_score -= rel_access_score + + # penalize stability directly (more stability than ground truth is not a problem) + stability_ratio = sh_rec_dat["stability_results"]["success_ratio"] + shape_score += stability_ratio + + return shape_score + + def optimize_object_col_shape(self, obj_h: str, col_shape_dir: str, method="coacd"): """ Run VHACD optimization for a specific object. Identify the optimal collision shape and save the result as the new default. @@ -1737,97 +1941,30 @@ def optimize_object_col_shape_vhacd(self, obj_h: str, col_shape_dir: str): self.compute_receptacle_stability(obj_h) self.compute_receptacle_access_metrics(obj_h, use_gt=True) self.compute_receptacle_access_metrics(obj_h, use_gt=False) - self.grid_search_vhacd_params(obj_h) + if method == "vhacd": + self.grid_search_vhacd_params(obj_h) + elif method == "coacd": + self.run_coacd_grid_search(obj_h) self.compute_gt_errors(obj_h) # time to select the best version best_shape_id = "pr0" - pr0_shape_score = ( - 1.0 - - self.gt_data[obj_h]["shape_test_results"]["pr0"][ - "normalized_raycast_error" - ] - ) + pr0_shape_score = self.compute_shape_score(obj_h, "pr0") + settings_key = method + "_settings" best_shape_score = pr0_shape_score shape_scores = {} - access_scores = {"gt": {}, "pr0": {}} - stab_ratios = {"gt": {}, "pr0": {}} - rel_access_scores = defaultdict(dict) - rel_stab_scores = defaultdict(dict) - for shape_id in self.gt_data[obj_h]["vhacd_settings"].keys(): - # compare shape metric + for shape_id in self.gt_data[obj_h][settings_key].keys(): + shape_score = self.compute_shape_score(obj_h, shape_id) + shape_scores[shape_id] = shape_score + # we only want significantly better shapes (10% or 0.1 score better threshold) if ( - self.gt_data[obj_h]["shape_test_results"][shape_id][ - "normalized_raycast_error" - ] - > self.gt_data[obj_h]["shape_test_results"]["pr0"][ - "normalized_raycast_error" - ] + shape_score > (best_shape_score + abs(best_shape_score) * 0.1) + and shape_score - best_shape_score > 0.1 ): - # worse metric performance than the default, skip it. - continue - - shape_score = ( - 1.0 - - self.gt_data[obj_h]["shape_test_results"][shape_id][ - "normalized_raycast_error" - ] - ) - - # compare rec metrics - for rec_name, rec_data in self.gt_data[obj_h]["receptacles"].items(): - for orig_shape_id in access_scores.keys(): - if rec_name not in access_scores[orig_shape_id]: - access_scores[orig_shape_id][rec_name] = rec_data[ - "shape_id_results" - ][orig_shape_id]["access_results"]["receptacle_access_score"] - if rec_name not in stab_ratios[orig_shape_id]: - stab_ratios[orig_shape_id][rec_name] = rec_data[ - "shape_id_results" - ][orig_shape_id]["stability_results"]["success_ratio"] - - sh_rec_dat = rec_data["shape_id_results"][shape_id] - rel_access_scores[shape_id][rec_name] = ( - access_scores["gt"][rec_name] - - sh_rec_dat["access_results"]["receptacle_access_score"] - ) - rel_stab_scores[shape_id][rec_name] = ( - stab_ratios["gt"][rec_name] - - sh_rec_dat["stability_results"]["success_ratio"] - ) - - if ( - access_scores["gt"][rec_name] < 0.15 - or stab_ratios["gt"][rec_name] < 0.5 - ): - "this receptacle is not good anyway, so skip it" - continue - - if rec_name not in rel_access_scores["pr0"]: - rel_access_scores["pr0"][rec_name] = ( - access_scores["gt"][rec_name] - access_scores["pr0"][rec_name] - ) - pr0_shape_score += rel_access_scores["pr0"][rec_name] - if best_shape_id == "pr0": - best_shape_score = pr0_shape_score - if rec_name not in rel_stab_scores["pr0"]: - rel_stab_scores["pr0"][rec_name] = ( - stab_ratios["gt"][rec_name] - stab_ratios["pr0"][rec_name] - ) - pr0_shape_score += rel_stab_scores["pr0"][rec_name] - if best_shape_id == "pr0": - best_shape_score = pr0_shape_score - - # TODO: weight these up? - shape_score += rel_stab_scores[shape_id][rec_name] - shape_score += rel_access_scores[shape_id][rec_name] - - shape_scores[shape_id] = shape_score - if shape_score > best_shape_score: best_shape_id = shape_id best_shape_score = shape_score - print(self.gt_data[obj_h]["vhacd_settings"]) + print(self.gt_data[obj_h][settings_key]) print(shape_scores) if best_shape_id != "pr0": @@ -1835,13 +1972,20 @@ def optimize_object_col_shape_vhacd(self, obj_h: str, col_shape_dir: str): print( f"Best shape_id = {best_shape_id} with shape score {best_shape_score} better than 'pr0' with shape score {pr0_shape_score}." ) - self.compute_vhacd_col_shape( - obj_h, self.gt_data[obj_h]["vhacd_settings"][best_shape_id][0] - ) - # copy the collision asset into the correct directory - obj_name = obj_h.split(".object_config.json")[0].split("/")[-1] - col_shape_path = os.path.join(col_shape_dir, obj_name + ".obj") - os.system(f"obj2gltf -i {col_shape_path} -o {cur_col_shape_path}") + if method == "vhacd": + self.compute_vhacd_col_shape( + obj_h, self.gt_data[obj_h][settings_key][best_shape_id][0] + ) + # copy the collision asset into the correct directory + obj_name = obj_h.split(".object_config.json")[0].split("/")[-1] + col_shape_path = os.path.join(col_shape_dir, obj_name + ".obj") + os.system(f"obj2gltf -i {col_shape_path} -o {cur_col_shape_path}") + elif method == "coacd": + asset_file, num_hulls = self.run_coacd( + obj_h, self.gt_data[obj_h][settings_key][best_shape_id][0] + ) + os.system(f"cp {asset_file} {cur_col_shape_path}") + else: print( f"Best shape_id = {best_shape_id} with shape score {best_shape_score}." @@ -1849,8 +1993,12 @@ def optimize_object_col_shape_vhacd(self, obj_h: str, col_shape_dir: str): best_shape_params = None if best_shape_id != "pr0": - best_shape_params = self.gt_data[obj_h]["vhacd_settings"][best_shape_id] + best_shape_params = self.gt_data[obj_h][settings_key][best_shape_id] + + # self.cache_global_results() self.clean_obj_gt(obj_h) + # then save results to file + # self.save_results_to_csv("cpo_out") return (best_shape_id, best_shape_score, pr0_shape_score, best_shape_params) def compute_vhacd_col_shape( @@ -2289,13 +2437,17 @@ def main(): # ---------------------------------------------------- # specific object handle - # 163cdd355d2d228880dd6ed6f3fc5b49f7ba538a - # 174ea8cf2a80b38b2ce4dbbda6de2fee33d3870b - # 5277bce5d54fba0ab5ee869ed623932b447457d4 - # obj_of_interest = "163cdd355d2d228880dd6ed6f3fc5b49f7ba538a" - # obj_of_interest = "174ea8cf2a80b38b2ce4dbbda6de2fee33d3870b" + # 392e00c6db2728b11495753fc32bb9df67e89acb + # 3c0114990e89b0c6a978793245301a99285e503a + # b0929827deff77bee9b5a1120d31b29ecc26ae1f + # c2ac5a249ca5897894ae36b08376d35fde7914a3 + # 827df9acf75c5a4ab0384b7c32c230a61fbcc9f6 + # obj_of_interest = "c2ac5a249ca5897894ae36b08376d35fde7914a3" + # obj_of_interest = "95f30d803f3373b011b8f10a0614c210b7c8bbe2" # obj_h = otm.get_file_template_handles(obj_of_interest)[0] - # results = cpo.optimize_object_col_shape_vhacd(obj_h,col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/") + # #results = cpo.optimize_object_col_shape_vhacd(obj_h,col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/") + # cpo.output_directory = None + # results = cpo.optimize_object_col_shape(obj_h,col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/",method="coacd") # print(f"opt_results = {results}") # exit() # ---------------------------------------------------- @@ -2343,13 +2495,14 @@ def main(): correct_object_orientations(all_handles, obj_orientations, cpo.mm) # ---------------------------------------------------- - # run VHACD opt for all shapes + # run shape opt for all shapes results = [] for obj_h in all_handles: results.append( - cpo.optimize_object_col_shape_vhacd( + cpo.optimize_object_col_shape( obj_h, col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/", + method="coacd", ) ) From 4458b538c7e36411f2847a11a417c31ef80c7443 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 19 Apr 2023 09:25:41 -0700 Subject: [PATCH 036/153] polish UI and add CLI --- tools/collision_shape_automation.py | 198 +++++++++++++++++----------- 1 file changed, 118 insertions(+), 80 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index e9699351c1..be9df56874 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1925,7 +1925,9 @@ def compute_shape_score(self, obj_h: str, shape_id: str) -> float: return shape_score - def optimize_object_col_shape(self, obj_h: str, col_shape_dir: str, method="coacd"): + def optimize_object_col_shape( + self, obj_h: str, col_shape_dir: Optional[str] = None, method="coacd" + ): """ Run VHACD optimization for a specific object. Identify the optimal collision shape and save the result as the new default. @@ -1942,6 +1944,9 @@ def optimize_object_col_shape(self, obj_h: str, col_shape_dir: str, method="coac self.compute_receptacle_access_metrics(obj_h, use_gt=True) self.compute_receptacle_access_metrics(obj_h, use_gt=False) if method == "vhacd": + assert ( + col_shape_dir is not None + ), "Must provide the directory of the VHACD collision shape output." self.grid_search_vhacd_params(obj_h) elif method == "coacd": self.run_coacd_grid_search(obj_h) @@ -2405,12 +2410,24 @@ def main(): description="Automate collision shape creation and validation." ) parser.add_argument("--dataset", type=str, help="path to SceneDataset.") + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument( + "--scenes", type=str, nargs="+", help="one or more scenes to optimize." + ) + group.add_argument( + "--objects", type=str, nargs="+", help="one or more objects to optimize." + ) parser.add_argument( "--output-dir", type=str, default="collision_shape_automation/", help="output directory for saved csv and images. Default = `./collision_shape_automation/`.", ) + parser.add_argument( + "--debug-images", + action="store_true", + help="turns on debug image output.", + ) args = parser.parse_args() sim_settings = default_sim_settings.copy() @@ -2421,98 +2438,119 @@ def main(): sim_settings["height"] = 720 sim_settings["clear_color"] = mn.Color4.magenta() * 0.5 - # one-off single object logic: - # evaluate_collision_shape(args.object_handle, sim_settings) - # use the CollisionProxyOptimizer to compute metrics for multiple objects cpo = CollisionProxyOptimizer(sim_settings, output_directory=args.output_dir) - cpo.generate_debug_images = True + cpo.generate_debug_images = args.debug_images otm = cpo.mm.object_template_manager # ---------------------------------------------------- - # get all object handles - # all_handles = otm.get_file_template_handles() - # cpo.compute_and_save_results_for_objects(all_handles) - # ---------------------------------------------------- + # specific object handle provided + if args.objects: + # deduplicate the list + unique_objects = list(dict.fromkeys(args.objects)) + + # validate the object handles + object_handles = [] + for object_name in unique_objects: + matching_templates = otm.get_file_template_handles(object_name) + assert ( + len(matching_templates) > 0 + ), f"No matching templates in the dataset for '{object_name}'" + assert ( + len(matching_templates) == 1 + ), f"More than one matching template in the dataset for '{object_name}': {matching_templates}" + object_handles.append(matching_templates[0]) - # ---------------------------------------------------- - # specific object handle - # 392e00c6db2728b11495753fc32bb9df67e89acb - # 3c0114990e89b0c6a978793245301a99285e503a - # b0929827deff77bee9b5a1120d31b29ecc26ae1f - # c2ac5a249ca5897894ae36b08376d35fde7914a3 - # 827df9acf75c5a4ab0384b7c32c230a61fbcc9f6 - # obj_of_interest = "c2ac5a249ca5897894ae36b08376d35fde7914a3" - # obj_of_interest = "95f30d803f3373b011b8f10a0614c210b7c8bbe2" - # obj_h = otm.get_file_template_handles(obj_of_interest)[0] - # #results = cpo.optimize_object_col_shape_vhacd(obj_h,col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/") - # cpo.output_directory = None - # results = cpo.optimize_object_col_shape(obj_h,col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/",method="coacd") - # print(f"opt_results = {results}") - # exit() + # optimize the objects + results = [] + for obj_h in object_handles: + results.append(cpo.optimize_object_col_shape(obj_h, method="coacd")) + + # display results + print("Object Optimization Results:") + for obj_h, obj_result in zip(object_handles, results): + print(f" {obj_h}: {obj_result}") # ---------------------------------------------------- # ---------------------------------------------------- - # run the pipeline for a set of scenes with separate output files for each - scenes_of_interest = ["102816036"] - # get all scenes from the mm - # scenes_of_interest = [ - # handle.split(".scene_instance.json")[0].split("/")[-1] - # for handle in cpo.mm.get_scene_handles() - # ] - - for scene_of_interest in scenes_of_interest: - cpo.init_caches() - # ---------------------------------------------------- - # get object handles from a specific scene - objects_in_scene = get_objects_in_scene( - dataset_path=args.dataset, scene_handle=scene_of_interest, mm=cpo.mm - ) - all_handles = objects_in_scene - # ---------------------------------------------------- - - # ---------------------------------------------------- - # get a subset with receptacles defined - all_handles = [ - all_handles[i] - for i in range(len(all_handles)) - if object_has_receptacles(all_handles[i], otm) - ] - # all_handles = [all_handles[0]] - print(f"Number of objects with receptacles = {len(all_handles)}") - # ---------------------------------------------------- - - # ---------------------------------------------------- - # load object orientation metadata - reorient_objects = False - if reorient_objects: - fp_models_metadata_file = ( - "/home/alexclegg/Documents/dev/fphab/fpModels_metadata.csv" + # run the pipeline for a set of object parsed from a scene + if args.scenes: + scene_object_handles: Dict[str, List[str]] = {} + + # deduplicate the list + unique_scenes = list(dict.fromkeys(args.scenes)) + + # first validate the scene names have a unique match + scene_handles = cpo.mm.get_scene_handles() + for scene_name in unique_scenes: + matching_scenes = [h for h in scene_handles if scene_name in h] + assert ( + len(matching_scenes) > 0 + ), f"No scenes found matching provided scene name '{scene_name}'." + assert ( + len(matching_scenes) == 1 + ), f"More than one scenes found matching provided scene name '{scene_name}': {matching_scenes}." + + # collect all the objects for all the scenes in advance + for scene_name in unique_scenes: + objects_in_scene = get_objects_in_scene( + dataset_path=args.dataset, scene_handle=scene_name, mm=cpo.mm ) - obj_orientations = parse_object_orientations_from_metadata_csv( - fp_models_metadata_file + assert ( + len(objects_in_scene) > 0 + ), f"No objects found in scene '{scene_name}'. Are you sure this is a valid scene?" + scene_object_handles[scene_name] = objects_in_scene + + # optimize each scene + all_scene_results: Dict[ + str, Dict[str, List[Tuple[str, float, float, Any]]] + ] = {} + for scene, objects_in_scene in scene_object_handles.items(): + # clear and re-initialize the caches between scenes to prevent memory overflow on large batches. + cpo.init_caches() + + # ---------------------------------------------------- + # get a subset of objects with receptacles defined + rec_obj_in_scene = [ + objects_in_scene[i] + for i in range(len(objects_in_scene)) + if object_has_receptacles(objects_in_scene[i], otm) + ] + print( + f"Number of objects in scene '{scene}' with receptacles = {len(rec_obj_in_scene)}" ) - correct_object_orientations(all_handles, obj_orientations, cpo.mm) - # ---------------------------------------------------- - - # run shape opt for all shapes - results = [] - for obj_h in all_handles: - results.append( - cpo.optimize_object_col_shape( - obj_h, - col_shape_dir="~/Documents/dev2/habitat-sim/data/VHACD_outputs/", - method="coacd", + # ---------------------------------------------------- + + # ---------------------------------------------------- + # load object orientation metadata + # BUG: Receptacles are not re-oriented by internal re-orientation transforms. Need to fix this... + # reorient_objects = False + # if reorient_objects: + # fp_models_metadata_file = ( + # "/home/alexclegg/Documents/dev/fphab/fpModels_metadata.csv" + # ) + # obj_orientations = parse_object_orientations_from_metadata_csv( + # fp_models_metadata_file + # ) + # correct_object_orientations(all_handles, obj_orientations, cpo.mm) + # ---------------------------------------------------- + + # run shape opt for all objects in the scene + scene_results: Dict[str, List[Tuple[str, float, float, Any]]] = {} + for obj_h in rec_obj_in_scene: + scene_results[obj_h] = cpo.optimize_object_col_shape( + obj_h, method="coacd" ) - ) - print("Finished all objects in the scene!") - for oix, obj_h in enumerate(all_handles): - print(f" Object Handle = {obj_h}:") - print(f" results = {results[oix]}") - # cpo.compute_and_save_results_for_objects( - # all_handles, output_filename=scene_of_interest + "_cpo_out" - # ) + all_scene_results[scene] = scene_results + + print("------------------------------------") + print(f"Finished optimization of scene '{scene}': \n {scene_results}") + print("------------------------------------") + + print("==========================================") + print(f"Finished optimization of all scenes: \n {all_scene_results}") + print("==========================================") if __name__ == "__main__": From 5a1c1fd78ac687c815dcf9bb76012d9cd0d9d557 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 19 Apr 2023 09:52:30 -0700 Subject: [PATCH 037/153] save coacd output for each shape_id for later copy instead of additional recomputation --- tools/collision_shape_automation.py | 31 +++++++++++++++++++---------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index be9df56874..be51a24190 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1680,7 +1680,7 @@ def permute_param_variations( return permutations - def run_coacd_grid_search(self, obj_template_handle: str): + def run_coacd_grid_search(self, obj_template_handle: str) -> None: """ Run grid search on relevant COACD params for an object. """ @@ -1704,25 +1704,32 @@ def run_coacd_grid_search(self, obj_template_handle: str): setattr(coacd_param, attr, val) setting_string += f" '{attr}'={val}" + self.increment_proxy_index(obj_template_handle) + shape_id = self.get_proxy_shape_id(obj_template_handle) + coacd_iteration_time = time.time() output_file, num_hulls = self.run_coacd( obj_template_handle, coacd_param ) + # setup the proxy otm = self.mm.object_template_manager obj_template = otm.get_template_by_handle(obj_template_handle) obj_template.collision_asset_handle = output_file otm.register_template(obj_template) - self.increment_proxy_index(obj_template_handle) - - shape_id = self.get_proxy_shape_id(obj_template_handle) if "coacd_settings" not in self.gt_data[obj_template_handle]: self.gt_data[obj_template_handle]["coacd_settings"] = {} self.gt_data[obj_template_handle]["coacd_settings"][shape_id] = ( coacd_param, setting_string, ) + # store the asset file for this shape_id + if "coacd_output_files" not in self.gt_data[obj_template_handle]: + self.gt_data[obj_template_handle]["coacd_output_files"] = {} + self.gt_data[obj_template_handle]["coacd_output_files"][ + shape_id + ] = output_file self.compute_proxy_metrics(obj_template_handle) # self.compute_grid_collision_times(obj_template_handle, subdivisions=1) @@ -1746,7 +1753,6 @@ def run_coacd_grid_search(self, obj_template_handle: str): print( f" {shape_id} - {settings[1]} - {coacd_iteration_times[shape_id]}" ) - print(coacd_num_hulls) def run_coacd( self, @@ -1762,7 +1768,13 @@ def run_coacd( obj_name = obj_template_handle.split(".object_config.json")[0].split("/")[ -1 ] - output_file = "COACD_output/" + obj_name + ".glb" + output_file = ( + "COACD_output/" + + obj_name + + "_" + + self.get_proxy_shape_id(obj_template_handle) + + ".glb" + ) os.makedirs(os.path.dirname(output_file), exist_ok=True) input_filepath = self.get_obj_render_mesh_filepath(obj_template_handle) # TODO: this seems dirty, maybe refactor: @@ -1977,20 +1989,17 @@ def optimize_object_col_shape( print( f"Best shape_id = {best_shape_id} with shape score {best_shape_score} better than 'pr0' with shape score {pr0_shape_score}." ) + # copy the collision asset into the dataset directory if method == "vhacd": self.compute_vhacd_col_shape( obj_h, self.gt_data[obj_h][settings_key][best_shape_id][0] ) - # copy the collision asset into the correct directory obj_name = obj_h.split(".object_config.json")[0].split("/")[-1] col_shape_path = os.path.join(col_shape_dir, obj_name + ".obj") os.system(f"obj2gltf -i {col_shape_path} -o {cur_col_shape_path}") elif method == "coacd": - asset_file, num_hulls = self.run_coacd( - obj_h, self.gt_data[obj_h][settings_key][best_shape_id][0] - ) + asset_file = self.gt_data[obj_h]["coacd_output_files"][best_shape_id] os.system(f"cp {asset_file} {cur_col_shape_path}") - else: print( f"Best shape_id = {best_shape_id} with shape score {best_shape_score}." From 2fb86d089770a5ba89f06f06e84e1cba204d45bc Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 19 Apr 2023 10:16:37 -0700 Subject: [PATCH 038/153] fix coverge of debug image flag --- tools/collision_shape_automation.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index be51a24190..ba767fed0a 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -790,7 +790,10 @@ def setup_obj_gt( ) ) ) - if self.output_directory is not None: + if ( + self.generate_debug_images + and self.output_directory is not None + ): # use DebugVisualizer to get 6-axis view of the object dvb = hab_debug_vis.DebugVisualizer( sim=sim, @@ -1209,7 +1212,7 @@ def compute_receptacle_stability( cfg = self.get_cfg_with_mm(scene=scene_name) with habitat_sim.Simulator(cfg) as sim: dvb: Optional[hab_debug_vis.DebugVisualizer] = None - if self.output_directory is not None: + if self.generate_debug_images and self.output_directory is not None: dvb = hab_debug_vis.DebugVisualizer( sim=sim, output_path=self.output_directory, @@ -1381,7 +1384,7 @@ def run_physics_settle_test(self, obj_handle): # use DebugVisualizer to get 6-axis view of the object dvb: Optional[hab_debug_vis.DebugVisualizer] = None - if self.output_directory is not None: + if self.generate_debug_images and self.output_directory is not None: dvb = hab_debug_vis.DebugVisualizer( sim=sim, output_path=self.output_directory, From bfc7d467e5fce60a0b0307c35a9bf77f321306c7 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 19 Apr 2023 23:13:21 -0700 Subject: [PATCH 039/153] exclude objects with CLI --- tools/collision_shape_automation.py | 37 +++++++++++++++++++++++++++-- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index ba767fed0a..77d54bbd3b 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -2429,6 +2429,12 @@ def main(): group.add_argument( "--objects", type=str, nargs="+", help="one or more objects to optimize." ) + parser.add_argument( + "--exclude", + type=str, + nargs="+", + help="one or more objects to exclude from optimization (e.g. if it inspires a crash in COACD).", + ) parser.add_argument( "--output-dir", type=str, @@ -2455,6 +2461,10 @@ def main(): cpo.generate_debug_images = args.debug_images otm = cpo.mm.object_template_manager + excluded_object_strings = [] + if args.exclude: + excluded_object_strings = args.exclude + # ---------------------------------------------------- # specific object handle provided if args.objects: @@ -2464,6 +2474,7 @@ def main(): # validate the object handles object_handles = [] for object_name in unique_objects: + # get templates matches matching_templates = otm.get_file_template_handles(object_name) assert ( len(matching_templates) > 0 @@ -2471,7 +2482,17 @@ def main(): assert ( len(matching_templates) == 1 ), f"More than one matching template in the dataset for '{object_name}': {matching_templates}" - object_handles.append(matching_templates[0]) + obj_h = matching_templates[0] + + # skip excluded objects + exclude_object = False + for ex_obj in excluded_object_strings: + if ex_obj in obj_h: + print(f"Excluding object {object_name}.") + exclude_object = True + break + if not exclude_object: + object_handles.append(obj_h) # optimize the objects results = [] @@ -2511,7 +2532,19 @@ def main(): assert ( len(objects_in_scene) > 0 ), f"No objects found in scene '{scene_name}'. Are you sure this is a valid scene?" - scene_object_handles[scene_name] = objects_in_scene + + # skip excluded objects + included_objects = [] + for obj_h in objects_in_scene: + exclude_object = False + for ex_obj in excluded_object_strings: + if ex_obj in obj_h: + exclude_object = True + print(f"Excluding object {obj_h}.") + break + if not exclude_object: + included_objects.append(obj_h) + scene_object_handles[scene_name] = included_objects # optimize each scene all_scene_results: Dict[ From 3bf6cd5148763b22de7dd068e0df7701e64060b2 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 20 Apr 2023 14:55:12 -0700 Subject: [PATCH 040/153] add ModelCategorizer workflow compatibility --- tools/collision_shape_automation.py | 42 +++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 77d54bbd3b..2520b75d49 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -2435,6 +2435,12 @@ def main(): nargs="+", help="one or more objects to exclude from optimization (e.g. if it inspires a crash in COACD).", ) + parser.add_argument( + "--exclude_files", + type=str, + nargs="+", + help="provide one or more files with objects to exclude from optimization (NOTE: txt file with one id on each line, object names may include prefix 'fpModel.' which will be stripped.).", + ) parser.add_argument( "--output-dir", type=str, @@ -2446,6 +2452,12 @@ def main(): action="store_true", help="turns on debug image output.", ) + parser.add_argument( + "--export-fp-model-ids", + type=str, + default="fp_model_ids.txt", + help="Intercept optimization to output a txt file with model ids for online model categorizer view.", + ) args = parser.parse_args() sim_settings = default_sim_settings.copy() @@ -2464,10 +2476,20 @@ def main(): excluded_object_strings = [] if args.exclude: excluded_object_strings = args.exclude + if args.exclude_files: + for filepath in args.exclude_files: + assert os.path.exists(filepath) + with open(filepath, "r") as f: + lines = [line.strip().split("fpModel.")[-1] for line in f.readlines()] + excluded_object_strings.extend(lines) + excluded_object_strings = list(dict.fromkeys(excluded_object_strings)) # ---------------------------------------------------- # specific object handle provided if args.objects: + assert ( + not args.export_fp_model_ids + ), "Feature not available for objects, only for scenes." # deduplicate the list unique_objects = list(dict.fromkeys(args.objects)) @@ -2546,6 +2568,26 @@ def main(): included_objects.append(obj_h) scene_object_handles[scene_name] = included_objects + if args.export_fp_model_ids: + # intercept optimization to instead export a txt file with model ids for import into the model categorizer tool + with open(args.export_fp_model_ids, "w") as f: + aggregated_object_ids = [] + for _, scene_objects in scene_object_handles.items(): + rec_obj_in_scene = [ + scene_objects[i] + for i in range(len(scene_objects)) + if object_has_receptacles(scene_objects[i], otm) + ] + aggregated_object_ids.extend(rec_obj_in_scene) + aggregated_object_ids = list(dict.fromkeys(aggregated_object_ids)) + for obj_h in aggregated_object_ids: + obj_name = obj_h.split(".object_config.json")[0].split("/")[-1] + # TODO: this will change once the Model Categorizer supports these + if "_part_" not in obj_name: + f.write("fpModel." + obj_name + "\n") + print(f"Export fpModel ids to {args.export_fp_model_ids}") + exit() + # optimize each scene all_scene_results: Dict[ str, Dict[str, List[Tuple[str, float, float, Any]]] From 33c471432722aa3fba41af62662b87bc89073967 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 21 Apr 2023 10:56:36 -0700 Subject: [PATCH 041/153] option to override the coacd threshold values from CLI --- tools/collision_shape_automation.py | 38 ++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 2520b75d49..695f6917e1 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1683,7 +1683,11 @@ def permute_param_variations( return permutations - def run_coacd_grid_search(self, obj_template_handle: str) -> None: + def run_coacd_grid_search( + self, + obj_template_handle: str, + param_range_override: Optional[Dict[str, List[Any]]] = None, + ) -> None: """ Run grid search on relevant COACD params for an object. """ @@ -1694,6 +1698,9 @@ def run_coacd_grid_search(self, obj_template_handle: str) -> None: "threshold": [0.04, 0.01], } + if param_range_override is not None: + param_ranges = param_range_override + permutations = self.permute_param_variations(param_ranges) coacd_start_time = time.time() @@ -1941,7 +1948,11 @@ def compute_shape_score(self, obj_h: str, shape_id: str) -> float: return shape_score def optimize_object_col_shape( - self, obj_h: str, col_shape_dir: Optional[str] = None, method="coacd" + self, + obj_h: str, + col_shape_dir: Optional[str] = None, + method="coacd", + param_range_override: Optional[Dict[str, List[Any]]] = None, ): """ Run VHACD optimization for a specific object. @@ -1964,7 +1975,7 @@ def optimize_object_col_shape( ), "Must provide the directory of the VHACD collision shape output." self.grid_search_vhacd_params(obj_h) elif method == "coacd": - self.run_coacd_grid_search(obj_h) + self.run_coacd_grid_search(obj_h, param_range_override) self.compute_gt_errors(obj_h) # time to select the best version @@ -2455,11 +2466,22 @@ def main(): parser.add_argument( "--export-fp-model-ids", type=str, - default="fp_model_ids.txt", help="Intercept optimization to output a txt file with model ids for online model categorizer view.", ) + parser.add_argument( + "--coacd-thresholds", + type=float, + nargs="+", + help="one or more coacd thresholds [0-1] (lower is more detailed) to search. If not provided, default are [0.04, 0.01].", + ) args = parser.parse_args() + param_range_overrides = None + if args.coacd_thresholds: + param_range_overrides = { + "threshold": args.coacd_thresholds, + } + sim_settings = default_sim_settings.copy() sim_settings["scene_dataset_config_file"] = args.dataset # necessary for debug rendering @@ -2519,7 +2541,11 @@ def main(): # optimize the objects results = [] for obj_h in object_handles: - results.append(cpo.optimize_object_col_shape(obj_h, method="coacd")) + results.append( + cpo.optimize_object_col_shape( + obj_h, method="coacd", param_range_override=param_range_overrides + ) + ) # display results print("Object Optimization Results:") @@ -2626,7 +2652,7 @@ def main(): scene_results: Dict[str, List[Tuple[str, float, float, Any]]] = {} for obj_h in rec_obj_in_scene: scene_results[obj_h] = cpo.optimize_object_col_shape( - obj_h, method="coacd" + obj_h, method="coacd", param_range_override=param_range_overrides ) all_scene_results[scene] = scene_results From 917a33553f697a908f1010e362c4b19461dc30d4 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 21 Apr 2023 11:40:41 -0700 Subject: [PATCH 042/153] add a file annotating object ids which cause caocd trouble to use as a general exclusion set --- troublesome_object_ids.txt | 1 + 1 file changed, 1 insertion(+) create mode 100644 troublesome_object_ids.txt diff --git a/troublesome_object_ids.txt b/troublesome_object_ids.txt new file mode 100644 index 0000000000..638c57610d --- /dev/null +++ b/troublesome_object_ids.txt @@ -0,0 +1 @@ +1d5a78b46d32bf41584c800a0dfa2536d7f0b395 From ec6584c388510fedf9ff1ebc905e691175d20d73 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 21 Apr 2023 12:05:41 -0700 Subject: [PATCH 043/153] add the option to optimize all objects with receptacles defined in the dataset --- tools/collision_shape_automation.py | 34 +++++++++++++++++++++++++---- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 695f6917e1..8ff19ffc64 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -2440,6 +2440,11 @@ def main(): group.add_argument( "--objects", type=str, nargs="+", help="one or more objects to optimize." ) + group.add_argument( + "--all-rec-objects", + action="store_true", + help="Optimize all objects in the dataset with receptacles.", + ) parser.add_argument( "--exclude", type=str, @@ -2447,7 +2452,7 @@ def main(): help="one or more objects to exclude from optimization (e.g. if it inspires a crash in COACD).", ) parser.add_argument( - "--exclude_files", + "--exclude-files", type=str, nargs="+", help="provide one or more files with objects to exclude from optimization (NOTE: txt file with one id on each line, object names may include prefix 'fpModel.' which will be stripped.).", @@ -2508,12 +2513,27 @@ def main(): # ---------------------------------------------------- # specific object handle provided - if args.objects: + if args.objects or args.all_rec_objects: assert ( not args.export_fp_model_ids ), "Feature not available for objects, only for scenes." - # deduplicate the list - unique_objects = list(dict.fromkeys(args.objects)) + + unique_objects = None + + if args.objects: + # deduplicate the list + unique_objects = list(dict.fromkeys(args.objects)) + elif args.all_rec_objects: + objects_in_dataset = otm.get_file_template_handles() + rec_obj_in_dataset = [ + objects_in_dataset[i] + for i in range(len(objects_in_dataset)) + if object_has_receptacles(objects_in_dataset[i], otm) + ] + print( + f"Number of objects in dataset with receptacles = {len(rec_obj_in_dataset)}" + ) + unique_objects = rec_obj_in_dataset # validate the object handles object_handles = [] @@ -2540,12 +2560,18 @@ def main(): # optimize the objects results = [] + obj_counter = 0 for obj_h in object_handles: + print("+++++++++++++++++++++++++") + print("+++++++++++++++++++++++++") + print(f"Optimizing '{obj_h}' : {obj_counter} of {len(object_handles)}") + print("+++++++++++++++++++++++++") results.append( cpo.optimize_object_col_shape( obj_h, method="coacd", param_range_override=param_range_overrides ) ) + obj_counter += 1 # display results print("Object Optimization Results:") From 3b1fa06875b3f7818f616e03ab1b4423a7f2e493 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 24 Apr 2023 08:26:55 -0700 Subject: [PATCH 044/153] start and end index CLI + exception handling and failure tracking output --- tools/collision_shape_automation.py | 59 +++++++++++++++++++++++++---- troublesome_object_ids.txt | 2 + 2 files changed, 53 insertions(+), 8 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 8ff19ffc64..e4ea5a1920 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -2428,6 +2428,17 @@ def correct_object_orientations( mm.object_template_manager.register_template(obj_template) +def write_failure_ids( + failures: List[Tuple[int, str, str]], filename="failures_out.txt" +) -> None: + """ + Write handles from failure tuples to file for use as exclusion or for follow-up investigation. + """ + with open(filename, "w") as file: + for f in failures: + file.write(f[1]) + + def main(): parser = argparse.ArgumentParser( description="Automate collision shape creation and validation." @@ -2445,6 +2456,18 @@ def main(): action="store_true", help="Optimize all objects in the dataset with receptacles.", ) + parser.add_argument( + "--start-ix", + default=-1, + type=int, + help="If optimizing all assets, provide a start index.", + ) + parser.add_argument( + "--end-ix", + default=-1, + type=int, + help="If optimizing all assets, provide an end index.", + ) parser.add_argument( "--exclude", type=str, @@ -2481,6 +2504,14 @@ def main(): ) args = parser.parse_args() + if not args.all_rec_objects: + assert ( + args.start_ix == -1 + ), "Can only provide start index for all objects optimization." + assert ( + args.end_ix == -1 + ), "Can only provide end index for all objects optimization." + param_range_overrides = None if args.coacd_thresholds: param_range_overrides = { @@ -2560,23 +2591,35 @@ def main(): # optimize the objects results = [] - obj_counter = 0 - for obj_h in object_handles: + failures = [] + start = args.start_ix if args.start_ix >= 0 else 0 + end = args.end_ix if args.end_ix >= 0 else len(object_handles) + assert end >= start, f"Start index ({start}) is lower than end index ({end})." + for obj_ix in range(start, end): + obj_h = object_handles[obj_ix] print("+++++++++++++++++++++++++") print("+++++++++++++++++++++++++") - print(f"Optimizing '{obj_h}' : {obj_counter} of {len(object_handles)}") + print(f"Optimizing '{obj_h}' : {obj_ix} of {len(object_handles)}") print("+++++++++++++++++++++++++") - results.append( - cpo.optimize_object_col_shape( - obj_h, method="coacd", param_range_override=param_range_overrides + try: + results.append( + cpo.optimize_object_col_shape( + obj_h, + method="coacd", + param_range_override=param_range_overrides, + ) ) - ) - obj_counter += 1 + except Exception as err: + failures.append((obj_ix, obj_h, err)) # display results print("Object Optimization Results:") for obj_h, obj_result in zip(object_handles, results): print(f" {obj_h}: {obj_result}") + print("Failures:") + for f in failures: + print(f" {f}") + write_failure_ids(failures) # ---------------------------------------------------- # ---------------------------------------------------- diff --git a/troublesome_object_ids.txt b/troublesome_object_ids.txt index 638c57610d..22bd7ba04b 100644 --- a/troublesome_object_ids.txt +++ b/troublesome_object_ids.txt @@ -1 +1,3 @@ 1d5a78b46d32bf41584c800a0dfa2536d7f0b395 +05980eee8561a3ebaf0753a2f14f5871611e693e +0928513ee59d54e84c3baef6fe2f6daa7c9339b3 From 4fe9a11ab20c4ba8da96876742d5101b5d06b2a8 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 24 Apr 2023 15:29:39 -0700 Subject: [PATCH 045/153] load rec data when cpo is not initialized --- examples/viewer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index d9815f77d3..fb94c6e6f8 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -655,7 +655,7 @@ def debug_draw(self): ): rec_color = mn.Color4.blue() - receptacle.debug_draw(self.sim, color=rec_color) + receptacle.debug_draw(self.sim, color=rec_color) # mouse raycast circle white = mn.Color4(mn.Vector3(1.0), 1.0) From 4238e48f1e82d7b054e3db4309ccfb69c74b0fe6 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 25 Apr 2023 11:05:28 -0700 Subject: [PATCH 046/153] add object sampling logic to the viewer app for scene investigation --- examples/viewer.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/examples/viewer.py b/examples/viewer.py index fb94c6e6f8..12aec53126 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -6,6 +6,7 @@ import json import math import os +import random import string import sys import time @@ -270,6 +271,23 @@ def __init__( self.reconfigure_sim() self.debug_semantic_colors = {} + self.clutter_object_set = [ + "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", + ] + self.clutter_object_instances = [] + # add some clutter objects to the MM + self.sim.metadata_mediator.object_template_manager.load_configs( + "data/objects/ycb/configs/" + ) + # compute NavMesh if not already loaded by the scene. if ( not self.sim.pathfinder.is_loaded From d15897d28dbd4e978d967a01893553e63eaf924f Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 25 Apr 2023 16:34:00 -0700 Subject: [PATCH 047/153] minor change to documentation and console output --- examples/viewer.py | 2 ++ tools/collision_shape_automation.py | 4 +++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index 12aec53126..220c222867 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1537,6 +1537,8 @@ def print_help_text(self) -> None: 'o': Toggle display of collision proxy shapes for the scene. (+SHIFT) Toggle display of original render shapes (and Receptacles). 't': CLI for modifying un-bound viewer parameters during runtime. + 'u': Sample an object placement from the currently selected object or receptacle. + (+SHIFT) Remove all previously sampled objects. ===================================================== """ diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index e4ea5a1920..d56c150d40 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -2609,9 +2609,11 @@ def main(): param_range_override=param_range_overrides, ) ) + print( + f"Completed optimization of '{obj_h}' : {obj_ix} of {len(object_handles)}" + ) except Exception as err: failures.append((obj_ix, obj_h, err)) - # display results print("Object Optimization Results:") for obj_h, obj_result in zip(object_handles, results): From def3dc43b7180e58a2c3a487a66cb966ae5ef613 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 26 Apr 2023 16:13:21 -0700 Subject: [PATCH 048/153] add CLI additional CLI options for part only and file defined objects --- tools/collision_shape_automation.py | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index d56c150d40..ec60495d4f 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -2456,6 +2456,11 @@ def main(): action="store_true", help="Optimize all objects in the dataset with receptacles.", ) + group.add_argument( + "--objects-file", + type=str, + help="optimize objects from a file containing object names separated by newline characters.", + ) parser.add_argument( "--start-ix", default=-1, @@ -2468,6 +2473,11 @@ def main(): type=int, help="If optimizing all assets, provide an end index.", ) + parser.add_argument( + "--parts-only", + action="store_true", + help="culls all objects without _part_ in the name.", + ) parser.add_argument( "--exclude", type=str, @@ -2544,7 +2554,7 @@ def main(): # ---------------------------------------------------- # specific object handle provided - if args.objects or args.all_rec_objects: + if args.objects or args.all_rec_objects or args.objects_file: assert ( not args.export_fp_model_ids ), "Feature not available for objects, only for scenes." @@ -2554,6 +2564,11 @@ def main(): if args.objects: # deduplicate the list unique_objects = list(dict.fromkeys(args.objects)) + elif args.objects_file: + assert os.path.exists(args.objects_file) + with open(args.objects_file, "r") as f: + lines = [line.strip() for line in f.readlines()] + unique_objects = list(dict.fromkeys(lines)) elif args.all_rec_objects: objects_in_dataset = otm.get_file_template_handles() rec_obj_in_dataset = [ @@ -2589,6 +2604,10 @@ def main(): if not exclude_object: object_handles.append(obj_h) + if args.parts_only: + object_handles = [obj_h for obj_h in object_handles if "_part_" in obj_h] + print(f"part objects only = {object_handles}") + # optimize the objects results = [] failures = [] From 97fabc1231b003af80fdd85743ff57ead384367c Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 27 Apr 2023 08:06:50 -0700 Subject: [PATCH 049/153] refactor viewer for faster rec drawing and collision optimization for optional coacd import --- examples/viewer.py | 2 +- tools/collision_shape_automation.py | 12 +++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 220c222867..fbf919b2a6 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -673,7 +673,7 @@ def debug_draw(self): ): rec_color = mn.Color4.blue() - receptacle.debug_draw(self.sim, color=rec_color) + receptacle.debug_draw(self.sim, color=rec_color) # mouse raycast circle white = mn.Color4(mn.Vector3(1.0), 1.0) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index ec60495d4f..5ed4cdb4e1 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -12,7 +12,14 @@ import time from typing import Any, Dict, List, Optional, Tuple -import coacd +coacd_imported = False +try: + import coacd + + coacd_imported = True +except Exception: + coacd_imported = False + print("Failed to import coacd, is it installed? Linux only: 'pip install coacd'") import trimesh # not adding this causes some failures in mesh import @@ -1774,6 +1781,9 @@ def run_coacd( Run COACD on an object given a set of parameters producing a file. If output_file is not provided, defaults to "COACD_output/obj_name.glb" where obj_name is truncated handle (filename, no path or file ending). """ + assert ( + coacd_imported + ), "coacd is not installed. Linux only: 'pip install coacd'." if output_file is None: obj_name = obj_template_handle.split(".object_config.json")[0].split("/")[ -1 From 499c5073322e6bbd5883ce8e1101ac4fa6c5e2f5 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 27 Apr 2023 10:59:41 -0700 Subject: [PATCH 050/153] remove VHACD integration --- tools/collision_shape_automation.py | 147 +--------------------------- 1 file changed, 3 insertions(+), 144 deletions(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 5ed4cdb4e1..f6e1101f7a 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1836,90 +1836,6 @@ def run_coacd( scene.export(output_file) return output_file, len(parts) - def grid_search_vhacd_params(self, obj_template_handle: str): - """ - For a specified set of search parameters, try all combinations by: - 1. computing new proxy shape - 2. evaluating new proxy shape across various metrics - """ - - # vhacd_test_params = habitat_sim.VHACDParameters() - - # vhacd_test_params.max_num_vertices_per_ch = 64 - # vhacd_test_params.max_convex_hulls = 1024 - # vhacd_test_params.plane_downsampling = 4 #1-16 - # vhacd_test_params.convex_hull_downsampling = 4 #1-16 - - # vhacd_test_params.alpha = 0.05 #bias towards symmetry [0-1] - # vhacd_test_params.beta = 0.05 #bias toward revolution axes [0-1] - - # vhacd_test_params.mode = 0 #0=voxel, 1=tetrahedral - # vhacd_test_params.pca = 0 #1=use PCA normalization - - param_ranges = { - # "pca": (0,1), #pca seems worse, no speed improvement - # "mode": (0,1), #tetrahedral mode seems worse - "alpha": (0.05, 0.1), - # "alpha": (0.05,), - "beta": (0.05, 0.1), - "plane_downsampling": [1], - "convex_hull_downsampling": [1], - # "max_num_vertices_per_ch": (16, 32), - "max_convex_hulls": (64, 128), - # "max_convex_hulls": (500,), - "resolution": [200000], - } - - permutations = self.permute_param_variations(param_ranges) - - vhacd_start_time = time.time() - vhacd_iteration_times = {} - # evaluate VHACD settings - for setting in permutations: - vhacd_params = habitat_sim.VHACDParameters() - setting_string = "" - for attr, val in setting: - setattr(vhacd_params, attr, val) - setting_string += f" '{attr}'={val}" - vhacd_iteration_time = time.time() - # create new shape and increment shape id - self.compute_vhacd_col_shape(obj_template_handle, vhacd_params) - self.increment_proxy_index(obj_template_handle) - - # cache vhacd settings - shape_id = self.get_proxy_shape_id(obj_template_handle) - if "vhacd_settings" not in self.gt_data[obj_template_handle]: - self.gt_data[obj_template_handle]["vhacd_settings"] = {} - self.gt_data[obj_template_handle]["vhacd_settings"][shape_id] = ( - vhacd_params, - setting_string, - ) - - # compute shape level metrics: - self.compute_proxy_metrics(obj_template_handle) - # self.compute_grid_collision_times(obj_template_handle, subdivisions=1) - # self.run_physics_settle_test(obj_template_handle) - # self.run_physics_sphere_shake_test(obj_template_handle) - - # compute receptacle metrics - if self.compute_receptacle_useability_metrics: - self.compute_receptacle_access_metrics( - obj_handle=obj_template_handle, use_gt=False - ) - self.compute_receptacle_stability( - obj_handle=obj_template_handle, use_gt=False - ) - vhacd_iteration_times[shape_id] = time.time() - vhacd_iteration_time - - print(f"Total VHACD time = {time.time()-vhacd_start_time}") - print(" Iteration times = ") - for shape_id, settings in self.gt_data[obj_template_handle][ - "vhacd_settings" - ].items(): - print( - f" {shape_id} - {settings[1]} - {vhacd_iteration_times[shape_id]}" - ) - def compute_shape_score(self, obj_h: str, shape_id: str) -> float: """ Compute the shape score for the given object and shape_id. @@ -1965,7 +1881,7 @@ def optimize_object_col_shape( param_range_override: Optional[Dict[str, List[Any]]] = None, ): """ - Run VHACD optimization for a specific object. + Run COACD optimization for a specific object. Identify the optimal collision shape and save the result as the new default. :return: Tuple(best_shape_id, best_shape_score, original_shape_score) if best_shape_id == "pr0", then optimization didn't change anything. @@ -1979,12 +1895,7 @@ def optimize_object_col_shape( self.compute_receptacle_stability(obj_h) self.compute_receptacle_access_metrics(obj_h, use_gt=True) self.compute_receptacle_access_metrics(obj_h, use_gt=False) - if method == "vhacd": - assert ( - col_shape_dir is not None - ), "Must provide the directory of the VHACD collision shape output." - self.grid_search_vhacd_params(obj_h) - elif method == "coacd": + if method == "coacd": self.run_coacd_grid_search(obj_h, param_range_override) self.compute_gt_errors(obj_h) @@ -2014,14 +1925,7 @@ def optimize_object_col_shape( f"Best shape_id = {best_shape_id} with shape score {best_shape_score} better than 'pr0' with shape score {pr0_shape_score}." ) # copy the collision asset into the dataset directory - if method == "vhacd": - self.compute_vhacd_col_shape( - obj_h, self.gt_data[obj_h][settings_key][best_shape_id][0] - ) - obj_name = obj_h.split(".object_config.json")[0].split("/")[-1] - col_shape_path = os.path.join(col_shape_dir, obj_name + ".obj") - os.system(f"obj2gltf -i {col_shape_path} -o {cur_col_shape_path}") - elif method == "coacd": + if method == "coacd": asset_file = self.gt_data[obj_h]["coacd_output_files"][best_shape_id] os.system(f"cp {asset_file} {cur_col_shape_path}") else: @@ -2039,50 +1943,6 @@ def optimize_object_col_shape( # self.save_results_to_csv("cpo_out") return (best_shape_id, best_shape_score, pr0_shape_score, best_shape_params) - def compute_vhacd_col_shape( - self, obj_template_handle: str, vhacd_params: habitat_sim.VHACDParameters - ) -> None: - """ - Compute a new VHACD convex decomposition for the object and set it as the active collision proxy. - """ - - new_template_handle = None - - otm = self.mm.object_template_manager - matching_obj_handles = otm.get_file_template_handles(obj_template_handle) - assert ( - len(matching_obj_handles) == 1 - ), f"None or many matching handles to substring `{obj_template_handle}`: {matching_obj_handles}" - obj_template = otm.get_template_by_handle(matching_obj_handles[0]) - render_asset = obj_template.render_asset_handle - render_asset_path = os.path.abspath(render_asset) - - cfg = self.get_cfg_with_mm() - with habitat_sim.Simulator(cfg) as sim: - new_template_handle = sim.apply_convex_hull_decomposition( - render_asset_path, vhacd_params, save_chd_to_obj=True - ) - - # set the collision asset - matching_vhacd_handles = otm.get_file_template_handles(new_template_handle) - assert ( - len(matching_vhacd_handles) == 1 - ), f"None or many matching VHACD handles to substring `{new_template_handle}`: {matching_vhacd_handles}" - - vhacd_template = otm.get_template_by_handle(matching_vhacd_handles[0]) - - # copy the file to glb - new_filename = vhacd_template.collision_asset_handle.split(".obj")[0] + ".glb" - command = ( - f"obj2gltf -i {vhacd_template.collision_asset_handle} -o {new_filename}" - ) - - os.system(command) - print(command) - - obj_template.collision_asset_handle = new_filename - otm.register_template(obj_template) - def cache_global_results(self) -> None: """ Cache the current global cumulative results. @@ -2310,7 +2170,6 @@ def compute_and_save_results_for_objects( self.compute_receptacle_access_metrics(obj_h, use_gt=True) print(" PR Receptacle Metrics:") self.compute_receptacle_access_metrics(obj_h, use_gt=False) - # self.grid_search_vhacd_params(obj_h) self.compute_gt_errors(obj_h) print_dict_structure(self.gt_data) self.cache_global_results() From fc437990b1d429ef10cc7c5556fc253742b70cc5 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 27 Apr 2023 14:41:34 -0700 Subject: [PATCH 051/153] refactor improve interactive object sampling --- examples/viewer.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index fbf919b2a6..e6f6b1ac83 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -6,7 +6,6 @@ import json import math import os -import random import string import sys import time @@ -19,6 +18,7 @@ import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle import magnum as mn import numpy as np +from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler from magnum import shaders, text from magnum.platform.glfw import Application @@ -282,6 +282,7 @@ def __init__( "010_potted_meat_can", "024_bowl", ] + self.clutter_object_handles = [] self.clutter_object_instances = [] # add some clutter objects to the MM self.sim.metadata_mediator.object_template_manager.load_configs( From b5422110481f2a885e32faf72b3f8f4392fb16b0 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 28 Apr 2023 16:29:28 -0700 Subject: [PATCH 052/153] Improve clutter generation utility in viewer --- examples/viewer.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/examples/viewer.py b/examples/viewer.py index e6f6b1ac83..97b9fe1231 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -271,6 +271,8 @@ def __init__( self.reconfigure_sim() self.debug_semantic_colors = {} + # ----------------------------------------- + # Clutter Generation Integration: self.clutter_object_set = [ "002_master_chef_can", "003_cracker_box", @@ -284,10 +286,14 @@ def __init__( ] self.clutter_object_handles = [] self.clutter_object_instances = [] + # cache initial states for classification of unstable objects + self.clutter_object_initial_states = [] + self.num_unstable_objects = 0 # add some clutter objects to the MM self.sim.metadata_mediator.object_template_manager.load_configs( "data/objects/ycb/configs/" ) + # ----------------------------------------- # compute NavMesh if not already loaded by the scene. if ( @@ -718,6 +724,17 @@ def draw_event( self.simulate_single_step = False if simulation_call is not None: simulation_call() + # compute object stability after physics step + self.num_unstable_objects = 0 + for obj_initial_state, obj in zip( + self.clutter_object_initial_states, self.clutter_object_instances + ): + translation_error = ( + obj_initial_state[0] - obj.translation + ).length() + if translation_error > 0.1: + self.num_unstable_objects += 1 + if global_call is not None: global_call() @@ -1464,6 +1481,7 @@ def draw_text(self, sensor_spec): Sensor Type: {sensor_type_string} Sensor Subtype: {sensor_subtype_string} Mouse Interaction Mode: {mouse_mode_string} +Unstable Objects: {self.num_unstable_objects} of {len(self.clutter_object_instances)} """ ) self.shader.draw(self.window_text.mesh) @@ -1540,6 +1558,7 @@ def print_help_text(self) -> None: 't': CLI for modifying un-bound viewer parameters during runtime. 'u': Sample an object placement from the currently selected object or receptacle. (+SHIFT) Remove all previously sampled objects. + (+ALT) Sample from all "active" unfiltered Receptacles. ===================================================== """ From daa6f728c66585bcd2755e2bf410ca40b7b64867 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 28 Apr 2023 18:41:51 -0700 Subject: [PATCH 053/153] add spot_viewer application for evaluating navigability --- examples/spot_viewer.py | 1039 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 1039 insertions(+) create mode 100644 examples/spot_viewer.py diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py new file mode 100644 index 0000000000..a35f77883b --- /dev/null +++ b/examples/spot_viewer.py @@ -0,0 +1,1039 @@ +# 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 ctypes +import math +import os +import string +import sys +import time +from typing import Any, Callable, Dict, List, Optional, Tuple + +flags = sys.getdlopenflags() +sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) + +import habitat.articulated_agents.robots.spot_robot as spot_robot +import magnum as mn +import numpy as np +from magnum import shaders, text +from magnum.platform.glfw import Application + +import habitat_sim +from habitat_sim import ReplayRenderer, ReplayRendererConfiguration +from habitat_sim.logging import LoggingContext, logger +from habitat_sim.utils.settings import default_sim_settings, make_cfg + + +class ExtractedBaseVelNonCylinderAction: + def __init__(self, sim, spot): + self._sim = sim + self.spot = spot + self.base_vel_ctrl = habitat_sim.physics.VelocityControl() + self.base_vel_ctrl.controlling_lin_vel = True + self.base_vel_ctrl.lin_vel_is_local = True + self.base_vel_ctrl.controlling_ang_vel = True + self.base_vel_ctrl.ang_vel_is_local = True + self._allow_dyn_slide = True + self._allow_back = True + self._longitudinal_lin_speed = 10.0 + self._lateral_lin_speed = 10.0 + self._ang_speed = 10.0 + self._navmesh_offset = [[0.0, 0.0], [0.25, 0.0], [-0.25, 0.0]] + self._enable_lateral_move = True + self._collision_threshold = 1e-5 + + def collision_check(self, trans, target_trans, target_rigid_state, compute_sliding): + """ + trans: the transformation of the current location of the robot + target_trans: the transformation of the target location of the robot given the center original Navmesh + target_rigid_state: the target state of the robot given the center original Navmesh + compute_sliding: if we want to compute sliding or not + """ + # Get the offset positions + num_check_cylinder = len(self._navmesh_offset) + nav_pos_3d = [np.array([xz[0], 0.0, xz[1]]) for xz in self._navmesh_offset] + cur_pos = [trans.transform_point(xyz) for xyz in nav_pos_3d] + goal_pos = [target_trans.transform_point(xyz) for xyz in nav_pos_3d] + + # For step filter of offset positions + end_pos = [] + for i in range(num_check_cylinder): + pos = self._sim.step_filter(cur_pos[i], goal_pos[i]) + # Sanitize the height + pos[1] = 0.0 + cur_pos[i][1] = 0.0 + goal_pos[i][1] = 0.0 + end_pos.append(pos) + + # Planar move distance clamped by NavMesh + move = [] + for i in range(num_check_cylinder): + move.append((end_pos[i] - goal_pos[i]).length()) + + # For detection of linear or angualr velocities + # There is a collision if the difference between the clamped NavMesh position and target position is too great for any point. + diff = len([v for v in move if v > self._collision_threshold]) + + if diff > 0: + # Wrap the move direction if we use sliding + # Find the largest diff moving direction, which means that there is a collision in that cylinder + if compute_sliding: + max_idx = np.argmax(move) + move_vec = end_pos[max_idx] - cur_pos[max_idx] + new_end_pos = trans.translation + move_vec + return True, mn.Matrix4.from_( + target_rigid_state.rotation.to_matrix(), new_end_pos + ) + return True, trans + else: + return False, target_trans + + def update_base(self, if_rotation): + """ + Update the base of the robot + if_rotation: if the robot is rotating or not + """ + # Get the control frequency + ctrl_freq = 60 + # Get the current transformation + trans = self.spot.sim_obj.transformation + # Get the current rigid state + rigid_state = habitat_sim.RigidState( + mn.Quaternion.from_matrix(trans.rotation()), trans.translation + ) + # Integrate to get target rigid state + target_rigid_state = self.base_vel_ctrl.integrate_transform( + 1 / ctrl_freq, rigid_state + ) + # Get the traget transformation based on the target rigid state + target_trans = mn.Matrix4.from_( + target_rigid_state.rotation.to_matrix(), + target_rigid_state.translation, + ) + # We do sliding only if we allow the robot to do sliding and current + # robot is not rotating + compute_sliding = self._allow_dyn_slide and not if_rotation + # Check if there is a collision + did_coll, new_target_trans = self.collision_check( + trans, target_trans, target_rigid_state, compute_sliding + ) + # Update the base + self.spot.sim_obj.transformation = new_target_trans + + if self.spot._base_type == "leg": + # Fix the leg joints + self.spot.leg_joint_pos = self.spot.params.leg_init_params + + def step(self, forward, lateral, angular): + """ + provide forward, lateral, and angular velocities as [-1,1]. + """ + longitudinal_lin_vel = forward + lateral_lin_vel = lateral + ang_vel = angular + longitudinal_lin_vel = ( + np.clip(longitudinal_lin_vel, -1, 1) * self._longitudinal_lin_speed + ) + lateral_lin_vel = np.clip(lateral_lin_vel, -1, 1) * self._lateral_lin_speed + ang_vel = np.clip(ang_vel, -1, 1) * self._ang_speed + if not self._allow_back: + longitudinal_lin_vel = np.maximum(longitudinal_lin_vel, 0) + + self.base_vel_ctrl.linear_velocity = mn.Vector3( + longitudinal_lin_vel, 0, -lateral_lin_vel + ) + self.base_vel_ctrl.angular_velocity = mn.Vector3(0, ang_vel, 0) + + if longitudinal_lin_vel != 0.0 or lateral_lin_vel != 0.0 or ang_vel != 0.0: + self.update_base(ang_vel != 0.0) + + +class HabitatSimInteractiveViewer(Application): + # the maximum number of chars displayable in the app window + # using the magnum text module. These chars are used to + # display the CPU/GPU usage data + MAX_DISPLAY_TEXT_CHARS = 256 + + # how much to displace window text relative to the center of the + # app window (e.g if you want the display text in the top left of + # the app window, you will displace the text + # window width * -TEXT_DELTA_FROM_CENTER in the x axis and + # window height * TEXT_DELTA_FROM_CENTER in the y axis, as the text + # position defaults to the middle of the app window) + TEXT_DELTA_FROM_CENTER = 0.49 + + # font size of the magnum in-window display text that displays + # CPU and GPU usage info + DISPLAY_FONT_SIZE = 16.0 + + def __init__(self, sim_settings: Dict[str, Any]) -> None: + self.sim_settings: Dict[str:Any] = sim_settings + + self.enable_batch_renderer: bool = self.sim_settings["enable_batch_renderer"] + self.num_env: int = ( + self.sim_settings["num_environments"] if self.enable_batch_renderer else 1 + ) + + # Compute environment camera resolution based on the number of environments to render in the window. + window_size: mn.Vector2 = ( + self.sim_settings["window_width"], + self.sim_settings["window_height"], + ) + + configuration = self.Configuration() + configuration.title = "Habitat Sim Interactive Viewer" + configuration.size = window_size + Application.__init__(self, configuration) + self.fps: float = 60.0 + + # Compute environment camera resolution based on the number of environments to render in the window. + grid_size: mn.Vector2i = ReplayRenderer.environment_grid_size(self.num_env) + camera_resolution: mn.Vector2 = mn.Vector2(self.framebuffer_size) / mn.Vector2( + grid_size + ) + self.sim_settings["width"] = camera_resolution[0] + self.sim_settings["height"] = camera_resolution[1] + + # draw Bullet debug line visualizations (e.g. collision meshes) + self.debug_bullet_draw = False + # draw active contact point debug line visualizations + self.contact_debug_draw = False + # cache most recently loaded URDF file for quick-reload + self.cached_urdf = "" + + # set up our movement map + key = Application.KeyEvent.Key + self.pressed = { + key.UP: False, + key.DOWN: False, + key.LEFT: False, + key.RIGHT: False, + key.A: False, + key.D: False, + key.S: False, + key.W: False, + key.X: False, + key.Z: False, + } + + # set up our movement key bindings map + key = Application.KeyEvent.Key + self.key_to_action = { + key.UP: "look_up", + key.DOWN: "look_down", + key.LEFT: "turn_left", + key.RIGHT: "turn_right", + key.A: "move_left", + key.D: "move_right", + key.S: "move_backward", + key.W: "move_forward", + key.X: "move_down", + key.Z: "move_up", + } + + # Load a TrueTypeFont plugin and open the font file + self.display_font = text.FontManager().load_and_instantiate("TrueTypeFont") + relative_path_to_font = "../data/fonts/ProggyClean.ttf" + self.display_font.open_file( + os.path.join(os.path.dirname(__file__), relative_path_to_font), + 13, + ) + + # Glyphs we need to render everything + self.glyph_cache = text.GlyphCache(mn.Vector2i(256)) + self.display_font.fill_glyph_cache( + self.glyph_cache, + string.ascii_lowercase + + string.ascii_uppercase + + string.digits + + ":-_+,.! %µ", + ) + + # magnum text object that displays CPU/GPU usage data in the app window + self.window_text = text.Renderer2D( + self.display_font, + self.glyph_cache, + HabitatSimInteractiveViewer.DISPLAY_FONT_SIZE, + text.Alignment.TOP_LEFT, + ) + self.window_text.reserve(HabitatSimInteractiveViewer.MAX_DISPLAY_TEXT_CHARS) + + # text object transform in window space is Projection matrix times Translation Matrix + # put text in top left of window + self.window_text_transform = mn.Matrix3.projection( + self.framebuffer_size + ) @ mn.Matrix3.translation( + mn.Vector2(self.framebuffer_size) + * mn.Vector2( + -HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + HabitatSimInteractiveViewer.TEXT_DELTA_FROM_CENTER, + ) + ) + self.shader = shaders.VectorGL2D() + + # make magnum text background transparent + mn.gl.Renderer.enable(mn.gl.Renderer.Feature.BLENDING) + mn.gl.Renderer.set_blend_function( + mn.gl.Renderer.BlendFunction.ONE, + mn.gl.Renderer.BlendFunction.ONE_MINUS_SOURCE_ALPHA, + ) + mn.gl.Renderer.set_blend_equation( + mn.gl.Renderer.BlendEquation.ADD, mn.gl.Renderer.BlendEquation.ADD + ) + + # variables that track app data and CPU/GPU usage + self.num_frames_to_track = 60 + + self.previous_mouse_point = None + + # toggle physics simulation on/off + self.simulating = True + + # toggle a single simulation step at the next opportunity if not + # simulating continuously. + self.simulate_single_step = False + + self.spot = None + self.spot_action = None + self.spot_forward = 0 + self.spot_lateral = 0 + self.spot_angular = 0 + self.camera_distance = 2.0 + self.camera_angles = mn.Vector2() + + # configure our simulator + self.cfg: Optional[habitat_sim.simulator.Configuration] = None + self.sim: Optional[habitat_sim.simulator.Simulator] = None + self.tiled_sims: list[habitat_sim.simulator.Simulator] = None + self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None + self.replay_renderer: Optional[ReplayRenderer] = None + self.reconfigure_sim() + + # compute NavMesh if not already loaded by the scene. + if self.cfg.sim_cfg.scene_id.lower() != "none": + self.navmesh_config_and_recompute() + + self.place_spot() + + self.time_since_last_simulation = 0.0 + LoggingContext.reinitialize_from_env() + logger.setLevel("INFO") + self.print_help_text() + + def place_spot(self): + if self.sim.pathfinder.is_loaded: + valid_spot_point = None + robot_turn_radius = 0.7 + max_attempts = 1000 + attempt = 0 + while valid_spot_point is None and attempt < max_attempts: + spot_point = self.sim.pathfinder.get_random_navigable_point() + if ( + self.sim.pathfinder.distance_to_closest_obstacle(spot_point) + >= robot_turn_radius + ): + valid_spot_point = spot_point + attempt += 1 + if valid_spot_point is not None: + self.spot.base_pos = valid_spot_point + + def draw_contact_debug(self): + """ + This method is called to render a debug line overlay displaying active contact points and normals. + Yellow lines show the contact distance along the normal and red lines show the contact normal at a fixed length. + """ + yellow = mn.Color4.yellow() + red = mn.Color4.red() + cps = self.sim.get_physics_contact_points() + self.sim.get_debug_line_render().set_line_width(1.5) + camera_position = self.render_camera.render_camera.node.absolute_translation + # only showing active contacts + active_contacts = (x for x in cps if x.is_active) + for cp in active_contacts: + # red shows the contact distance + self.sim.get_debug_line_render().draw_transformed_line( + cp.position_on_b_in_ws, + cp.position_on_b_in_ws + + cp.contact_normal_on_b_in_ws * -cp.contact_distance, + red, + ) + # yellow shows the contact normal at a fixed length for visualization + self.sim.get_debug_line_render().draw_transformed_line( + cp.position_on_b_in_ws, + # + cp.contact_normal_on_b_in_ws * cp.contact_distance, + cp.position_on_b_in_ws + cp.contact_normal_on_b_in_ws * 0.1, + yellow, + ) + self.sim.get_debug_line_render().draw_circle( + translation=cp.position_on_b_in_ws, + radius=0.005, + color=yellow, + normal=camera_position - cp.position_on_b_in_ws, + ) + + def debug_draw(self): + """ + Additional draw commands to be called during draw_event. + """ + if self.debug_bullet_draw: + render_cam = self.render_camera.render_camera + proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) + self.sim.physics_debug_draw(proj_mat) + if self.contact_debug_draw: + self.draw_contact_debug() + + def draw_event( + self, + simulation_call: Optional[Callable] = None, + global_call: Optional[Callable] = None, + active_agent_id_and_sensor_name: Tuple[int, str] = (0, "color_sensor"), + ) -> None: + """ + Calls continuously to re-render frames and swap the two frame buffers + at a fixed rate. + """ + agent_acts_per_sec = self.fps + + mn.gl.default_framebuffer.clear( + mn.gl.FramebufferClear.COLOR | mn.gl.FramebufferClear.DEPTH + ) + + # Agent actions should occur at a fixed rate per second + self.time_since_last_simulation += Timer.prev_frame_duration + num_agent_actions: int = self.time_since_last_simulation * agent_acts_per_sec + self.move_and_look(int(num_agent_actions)) + + # Occasionally a frame will pass quicker than 1/60 seconds + if self.time_since_last_simulation >= 1.0 / self.fps: + if self.simulating or self.simulate_single_step: + self.sim.step_world(1.0 / self.fps) + self.simulate_single_step = False + if simulation_call is not None: + simulation_call() + if global_call is not None: + global_call() + + # reset time_since_last_simulation, accounting for potential overflow + self.time_since_last_simulation = math.fmod( + self.time_since_last_simulation, 1.0 / self.fps + ) + + keys = active_agent_id_and_sensor_name + + # set agent position relative to spot + x_rot = mn.Quaternion.rotation( + mn.Rad(self.camera_angles[0]), mn.Vector3(1, 0, 0) + ) + y_rot = mn.Quaternion.rotation( + mn.Rad(self.camera_angles[1]), mn.Vector3(0, 1, 0) + ) + local_camera_vec = mn.Vector3(0, 0, 1) + local_camera_position = y_rot.transform_vector( + x_rot.transform_vector(local_camera_vec * self.camera_distance) + ) + camera_position = local_camera_position + self.spot.base_pos + self.default_agent.scene_node.transformation = mn.Matrix4.look_at( + camera_position, + self.spot.base_pos, + mn.Vector3(0, 1, 0), + ) + + if self.enable_batch_renderer: + self.render_batch() + else: + self.sim._Simulator__sensors[keys[0]][keys[1]].draw_observation() + agent = self.sim.get_agent(keys[0]) + self.render_camera = agent.scene_node.node_sensor_suite.get(keys[1]) + self.debug_draw() + self.render_camera.render_target.blit_rgba_to_default() + + # draw CPU/GPU usage data and other info to the app window + mn.gl.default_framebuffer.bind() + self.draw_text(self.render_camera.specification()) + + self.swap_buffers() + Timer.next_frame() + self.redraw() + + def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: + """ + Set up our own agent and agent controls + """ + make_action_spec = habitat_sim.agent.ActionSpec + make_actuation_spec = habitat_sim.agent.ActuationSpec + MOVE, LOOK = 0.07, 1.5 + + # all of our possible actions' names + action_list = [ + "move_left", + "turn_left", + "move_right", + "turn_right", + "move_backward", + "look_up", + "move_forward", + "look_down", + "move_down", + "move_up", + ] + + action_space: Dict[str, habitat_sim.agent.ActionSpec] = {} + + # build our action space map + for action in action_list: + actuation_spec_amt = MOVE if "move" in action else LOOK + action_spec = make_action_spec( + action, make_actuation_spec(actuation_spec_amt) + ) + action_space[action] = action_spec + + sensor_spec: List[habitat_sim.sensor.SensorSpec] = self.cfg.agents[ + self.agent_id + ].sensor_specifications + + agent_config = habitat_sim.agent.AgentConfiguration( + height=1.5, + radius=0.1, + sensor_specifications=sensor_spec, + action_space=action_space, + body_type="cylinder", + ) + return agent_config + + def reconfigure_sim(self) -> None: + """ + Utilizes the current `self.sim_settings` to configure and set up a new + `habitat_sim.Simulator`, and then either starts a simulation instance, or replaces + the current simulator instance, reloading the most recently loaded scene + """ + # configure our sim_settings but then set the agent to our default + self.cfg = make_cfg(self.sim_settings) + self.agent_id: int = self.sim_settings["default_agent"] + self.cfg.agents[self.agent_id] = self.default_agent_config() + + if self.enable_batch_renderer: + self.cfg.enable_batch_renderer = True + self.cfg.sim_cfg.create_renderer = False + self.cfg.sim_cfg.enable_gfx_replay_save = True + + if self.sim_settings["stage_requires_lighting"]: + logger.info("Setting synthetic lighting override for stage.") + self.cfg.sim_cfg.override_scene_light_defaults = True + self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY + + if self.sim is None: + self.tiled_sims = [] + for _i in range(self.num_env): + self.tiled_sims.append(habitat_sim.Simulator(self.cfg)) + self.sim = self.tiled_sims[0] + else: # edge case + for i in range(self.num_env): + if ( + self.tiled_sims[i].config.sim_cfg.scene_id + == self.cfg.sim_cfg.scene_id + ): + # we need to force a reset, so change the internal config scene name + self.tiled_sims[i].config.sim_cfg.scene_id = "NONE" + self.tiled_sims[i].reconfigure(self.cfg) + + # post reconfigure + self.default_agent = self.sim.get_agent(self.agent_id) + self.render_camera = self.default_agent.scene_node.node_sensor_suite.get( + "color_sensor" + ) + + # set sim_settings scene name as actual loaded scene + self.sim_settings["scene"] = self.sim.curr_scene_name + + # Initialize replay renderer + if self.enable_batch_renderer and self.replay_renderer is None: + self.replay_renderer_cfg = ReplayRendererConfiguration() + self.replay_renderer_cfg.num_environments = self.num_env + self.replay_renderer_cfg.standalone = ( + False # Context is owned by the GLFW window + ) + self.replay_renderer_cfg.sensor_specifications = self.cfg.agents[ + self.agent_id + ].sensor_specifications + self.replay_renderer_cfg.gpu_device_id = self.cfg.sim_cfg.gpu_device_id + self.replay_renderer_cfg.force_separate_semantic_scene_graph = False + self.replay_renderer_cfg.leave_context_with_background_renderer = False + self.replay_renderer = ReplayRenderer.create_batch_replay_renderer( + self.replay_renderer_cfg + ) + # Pre-load composite files + if sim_settings["composite_files"] is not None: + for composite_file in sim_settings["composite_files"]: + self.replay_renderer.preload_file(composite_file) + + # add the robot to the world via the wrapper + robot_path = "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" + self.spot = spot_robot.SpotRobot(robot_path, self.sim, fixed_base=True) + self.spot.reconfigure() + self.spot.update() + self.spot_action = ExtractedBaseVelNonCylinderAction(self.sim, self.spot) + + Timer.start() + self.step = -1 + + def render_batch(self): + """ + This method updates the replay manager with the current state of environments and renders them. + """ + for i in range(self.num_env): + # Apply keyframe + keyframe = self.tiled_sims[i].gfx_replay_manager.extract_keyframe() + self.replay_renderer.set_environment_keyframe(i, keyframe) + # Copy sensor transforms + sensor_suite = self.tiled_sims[i]._sensors + for sensor_uuid, sensor in sensor_suite.items(): + transform = sensor._sensor_object.node.absolute_transformation() + self.replay_renderer.set_sensor_transform(i, sensor_uuid, transform) + # Render + self.replay_renderer.render(mn.gl.default_framebuffer) + + def move_and_look(self, repetitions: int) -> None: + """ + This method is called continuously with `self.draw_event` to monitor + any changes in the movement keys map `Dict[KeyEvent.key, Bool]`. + When a key in the map is set to `True` the corresponding action is taken. + """ + # avoids unnecessary updates to grabber's object position + if repetitions == 0: + return + + key = Application.KeyEvent.Key + press: Dict[key.key, bool] = self.pressed + + inc = 0.02 + min_val = 0.1 + + if (press[key.UP] or press[key.W]) and not (press[key.DOWN] or press[key.S]): + self.spot_forward = max(min_val, self.spot_forward + inc) + elif not (press[key.UP] or press[key.W]) and (press[key.DOWN] or press[key.S]): + self.spot_forward = min(-min_val, self.spot_forward - inc) + else: + self.spot_forward /= 2.0 + if abs(self.spot_forward) < min_val: + self.spot_forward = 0 + + if press[key.LEFT] and not press[key.RIGHT]: + self.spot_lateral = max(min_val, self.spot_lateral + inc) + elif press[key.RIGHT] and not press[key.LEFT]: + self.spot_lateral = min(-min_val, self.spot_lateral - inc) + else: + self.spot_lateral /= 2.0 + if abs(self.spot_lateral) < min_val: + self.spot_lateral = 0 + + if press[key.A] and not press[key.D]: + self.spot_angular = max(min_val, self.spot_angular + inc) + elif press[key.D] and not press[key.A]: + self.spot_angular = min(-min_val, self.spot_angular - inc) + else: + self.spot_angular /= 2.0 + if abs(self.spot_angular) < min_val: + self.spot_angular = 0 + + self.spot_action.step( + forward=self.spot_forward, + lateral=self.spot_lateral, + angular=self.spot_angular, + ) + + def invert_gravity(self) -> None: + """ + Sets the gravity vector to the negative of it's previous value. This is + a good method for testing simulation functionality. + """ + gravity: mn.Vector3 = self.sim.get_gravity() * -1 + self.sim.set_gravity(gravity) + + def key_press_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key press by performing the corresponding functions. + If the key pressed is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the + key will be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + pressed = Application.KeyEvent.Key + mod = Application.InputEvent.Modifier + + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) + # warning: ctrl doesn't always pass through with other key-presses + + if key == pressed.ESC: + event.accepted = True + self.exit_event(Application.ExitEvent) + return + + elif key == pressed.H: + self.print_help_text() + + elif key == pressed.TAB: + pass + + elif key == pressed.SPACE: + if not self.sim.config.sim_cfg.enable_physics: + logger.warn("Warning: physics was not enabled during setup") + else: + self.simulating = not self.simulating + logger.info(f"Command: physics simulating set to {self.simulating}") + + elif key == pressed.PERIOD: + if self.simulating: + logger.warn("Warning: physics simulation already running") + else: + self.simulate_single_step = True + logger.info("Command: physics step taken") + + elif key == pressed.COMMA: + self.debug_bullet_draw = not self.debug_bullet_draw + logger.info(f"Command: toggle Bullet debug draw: {self.debug_bullet_draw}") + + elif key == pressed.C: + if shift_pressed: + self.contact_debug_draw = not self.contact_debug_draw + logger.info( + f"Command: toggle contact debug draw: {self.contact_debug_draw}" + ) + else: + # perform a discrete collision detection pass and enable contact debug drawing to visualize the results + logger.info( + "Command: perform discrete collision detection and visualize active contacts." + ) + self.sim.perform_discrete_collision_detection() + self.contact_debug_draw = True + # TODO: add a nice log message with concise contact pair naming. + + elif key == pressed.T: + pass + + elif key == pressed.V: + self.invert_gravity() + logger.info("Command: gravity inverted") + + elif key == pressed.N: + # (default) - toggle navmesh visualization + # NOTE: (+ALT) - re-sample the agent position on the NavMesh + # NOTE: (+SHIFT) - re-compute the NavMesh + if alt_pressed: + logger.info("Command: resample agent state from navmesh") + self.place_spot() + elif shift_pressed: + logger.info("Command: recompute navmesh") + self.navmesh_config_and_recompute() + else: + if self.sim.pathfinder.is_loaded: + self.sim.navmesh_visualization = not self.sim.navmesh_visualization + logger.info("Command: toggle navmesh") + else: + logger.warn("Warning: recompute navmesh first") + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = True + event.accepted = True + self.redraw() + + def key_release_event(self, event: Application.KeyEvent) -> None: + """ + Handles `Application.KeyEvent` on a key release. When a key is released, if it + is part of the movement keys map `Dict[KeyEvent.key, Bool]`, then the key will + be set to False for the next `self.move_and_look()` to update the current actions. + """ + key = event.key + + # update map of moving/looking keys which are currently pressed + if key in self.pressed: + self.pressed[key] = False + event.accepted = True + self.redraw() + + def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: + """ + Handles `Application.MouseMoveEvent`. When in LOOK mode, enables the left + mouse button to steer the agent's facing direction. When in GRAB mode, + continues to update the grabber's object position with our agents position. + """ + button = Application.MouseMoveEvent.Buttons + # if interactive mode -> LOOK MODE + if event.buttons == button.LEFT: + self.camera_angles[0] -= float(event.relative_position[1]) * 0.01 + self.camera_angles[1] -= float(event.relative_position[0]) * 0.01 + self.camera_angles[0] = max(-3.13, min(0.5, self.camera_angles[0])) + self.camera_angles[1] = math.fmod(self.camera_angles[1], math.pi * 2) + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def mouse_press_event(self, event: Application.MouseEvent) -> None: + """ + Handles `Application.MouseEvent`. When in GRAB mode, click on + objects to drag their position. (right-click for fixed constraints) + """ + # button = Application.MouseEvent.Button + # physics_enabled = self.sim.get_physics_simulation_library() + + self.previous_mouse_point = self.get_mouse_position(event.position) + self.redraw() + event.accepted = True + + def mouse_scroll_event(self, event: Application.MouseScrollEvent) -> None: + """ + Handles `Application.MouseScrollEvent`. When in LOOK mode, enables camera + zooming (fine-grained zoom using shift) When in GRAB mode, adjusts the depth + of the grabber's object. (larger depth change rate using shift) + """ + scroll_mod_val = ( + event.offset.y + if abs(event.offset.y) > abs(event.offset.x) + else event.offset.x + ) + if not scroll_mod_val: + return + + # use shift to scale action response + shift_pressed = bool(event.modifiers & Application.InputEvent.Modifier.SHIFT) + # alt_pressed = bool(event.modifiers & Application.InputEvent.Modifier.ALT) + # ctrl_pressed = bool(event.modifiers & Application.InputEvent.Modifier.CTRL) + + # LOOK MODE + # use shift for fine-grained zooming + mod_val = 0.3 if shift_pressed else 0.15 + scroll_delta = scroll_mod_val * mod_val + self.camera_distance -= scroll_delta + + self.redraw() + event.accepted = True + + def mouse_release_event(self, event: Application.MouseEvent) -> None: + """ + Release any existing constraints. + """ + event.accepted = True + + def get_mouse_position(self, mouse_event_position: mn.Vector2i) -> mn.Vector2i: + """ + This function will get a screen-space mouse position appropriately + scaled based on framebuffer size and window size. Generally these would be + the same value, but on certain HiDPI displays (Retina displays) they may be + different. + """ + scaling = mn.Vector2i(self.framebuffer_size) / mn.Vector2i(self.window_size) + return mouse_event_position * scaling + + def navmesh_config_and_recompute(self) -> None: + """ + This method is setup to be overridden in for setting config accessibility + in inherited classes. + """ + self.navmesh_settings = habitat_sim.NavMeshSettings() + self.navmesh_settings.set_defaults() + self.navmesh_settings.agent_height = self.cfg.agents[self.agent_id].height + self.navmesh_settings.agent_radius = 0.3 + self.sim.recompute_navmesh( + self.sim.pathfinder, + self.navmesh_settings, + include_static_objects=True, + ) + + def exit_event(self, event: Application.ExitEvent): + """ + Overrides exit_event to properly close the Simulator before exiting the + application. + """ + for i in range(self.num_env): + self.tiled_sims[i].close(destroy=True) + event.accepted = True + exit(0) + + def draw_text(self, sensor_spec): + self.shader.bind_vector_texture(self.glyph_cache.texture) + self.shader.transformation_projection_matrix = self.window_text_transform + self.shader.color = [1.0, 1.0, 1.0] + + sensor_type_string = str(sensor_spec.sensor_type.name) + sensor_subtype_string = str(sensor_spec.sensor_subtype.name) + self.window_text.render( + f""" +{self.fps} FPS +Sensor Type: {sensor_type_string} +Sensor Subtype: {sensor_subtype_string} + """ + ) + self.shader.draw(self.window_text.mesh) + + def print_help_text(self) -> None: + """ + Print the Key Command help text. + """ + logger.info( + """ +===================================================== +Welcome to the Habitat-sim Python Spot Viewer application! +===================================================== +Mouse Functions +---------------- +In LOOK mode (default): + LEFT: + Click and drag to rotate the view around Spot. + WHEEL: + Zoom in and out on Spot view. + + +Key Commands: +------------- + esc: Exit the application. + 'h': Display this help message. + + Agent Controls: + 'wasd': Move Spot's body forward/backward and rotate left/right. + arrow keys: Move Spot's body forward/backwards and strafe left/right. + + Utilities: + 'r': Reset the simulator with the most recently loaded scene. + 'n': Show/hide NavMesh wireframe. + (+SHIFT) Recompute NavMesh with Spot settings (already done). + (+ALT) Re-sample Spot's position from the NavMesh. + ',': Render a Bullet collision shape debug wireframe overlay (white=active, green=sleeping, blue=wants sleeping, red=can't sleep). + 'c': Run a discrete collision detection pass and render a debug wireframe overlay showing active contact points and normals (yellow=fixed length normals, red=collision distances). + (+SHIFT) Toggle the contact point debug render overlay on/off. + + Object Interactions: + SPACE: Toggle physics simulation on/off. + '.': Take a single simulation step if not simulating continuously. +===================================================== +""" + ) + + +class Timer: + """ + Timer class used to keep track of time between buffer swaps + and guide the display frame rate. + """ + + start_time = 0.0 + prev_frame_time = 0.0 + prev_frame_duration = 0.0 + running = False + + @staticmethod + def start() -> None: + """ + Starts timer and resets previous frame time to the start time. + """ + Timer.running = True + Timer.start_time = time.time() + Timer.prev_frame_time = Timer.start_time + Timer.prev_frame_duration = 0.0 + + @staticmethod + def stop() -> None: + """ + Stops timer and erases any previous time data, resetting the timer. + """ + Timer.running = False + Timer.start_time = 0.0 + Timer.prev_frame_time = 0.0 + Timer.prev_frame_duration = 0.0 + + @staticmethod + def next_frame() -> None: + """ + Records previous frame duration and updates the previous frame timestamp + to the current time. If the timer is not currently running, perform nothing. + """ + if not Timer.running: + return + Timer.prev_frame_duration = time.time() - Timer.prev_frame_time + Timer.prev_frame_time = time.time() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + + # optional arguments + parser.add_argument( + "--scene", + default="./data/test_assets/scenes/simple_room.glb", + type=str, + help='scene/stage file to load (default: "./data/test_assets/scenes/simple_room.glb")', + ) + parser.add_argument( + "--dataset", + default="./data/objects/ycb/ycb.scene_dataset_config.json", + type=str, + metavar="DATASET", + help='dataset configuration file to use (default: "./data/objects/ycb/ycb.scene_dataset_config.json")', + ) + parser.add_argument( + "--disable-physics", + action="store_true", + help="disable physics simulation (default: False)", + ) + parser.add_argument( + "--stage-requires-lighting", + action="store_true", + help="Override configured lighting to use synthetic lighting for the stage.", + ) + parser.add_argument( + "--enable-batch-renderer", + action="store_true", + help="Enable batch rendering mode. The number of concurrent environments is specified with the num-environments parameter.", + ) + parser.add_argument( + "--num-environments", + default=1, + type=int, + help="Number of concurrent environments to batch render. Note that only the first environment simulates physics and can be controlled.", + ) + parser.add_argument( + "--composite-files", + type=str, + nargs="*", + help="Composite files that the batch renderer will use in-place of simulation assets to improve memory usage and performance. If none is specified, the original scene files will be loaded from disk.", + ) + parser.add_argument( + "--width", + default=800, + type=int, + help="Horizontal resolution of the window.", + ) + parser.add_argument( + "--height", + default=600, + type=int, + help="Vertical resolution of the window.", + ) + + args = parser.parse_args() + + if args.num_environments < 1: + parser.error("num-environments must be a positive non-zero integer.") + if args.width < 1: + parser.error("width must be a positive non-zero integer.") + if args.height < 1: + parser.error("height must be a positive non-zero integer.") + + # Setting up sim_settings + sim_settings: Dict[str, Any] = default_sim_settings + sim_settings["scene"] = args.scene + sim_settings["scene_dataset_config_file"] = args.dataset + sim_settings["enable_physics"] = not args.disable_physics + sim_settings["stage_requires_lighting"] = args.stage_requires_lighting + sim_settings["enable_batch_renderer"] = args.enable_batch_renderer + sim_settings["num_environments"] = args.num_environments + sim_settings["composite_files"] = args.composite_files + sim_settings["window_width"] = args.width + sim_settings["window_height"] = args.height + sim_settings["sensor_height"] = 0 + + # start the application + HabitatSimInteractiveViewer(sim_settings).exec() From cca25962fae0552340f4d5bd2d0a6ceef3505341 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 28 Apr 2023 18:46:54 -0700 Subject: [PATCH 054/153] minor useability fixes for spot_viewer --- examples/spot_viewer.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index a35f77883b..4552b89af6 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -324,15 +324,11 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: def place_spot(self): if self.sim.pathfinder.is_loaded: valid_spot_point = None - robot_turn_radius = 0.7 max_attempts = 1000 attempt = 0 while valid_spot_point is None and attempt < max_attempts: spot_point = self.sim.pathfinder.get_random_navigable_point() - if ( - self.sim.pathfinder.distance_to_closest_obstacle(spot_point) - >= robot_turn_radius - ): + if self.sim.pathfinder.distance_to_closest_obstacle(spot_point) >= 0.25: valid_spot_point = spot_point attempt += 1 if valid_spot_point is not None: @@ -763,7 +759,7 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: if event.buttons == button.LEFT: self.camera_angles[0] -= float(event.relative_position[1]) * 0.01 self.camera_angles[1] -= float(event.relative_position[0]) * 0.01 - self.camera_angles[0] = max(-3.13, min(0.5, self.camera_angles[0])) + self.camera_angles[0] = max(-1.55, min(0.5, self.camera_angles[0])) self.camera_angles[1] = math.fmod(self.camera_angles[1], math.pi * 2) self.previous_mouse_point = self.get_mouse_position(event.position) From f0e8a791fc7c217f994683638d807094de2657a9 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 1 May 2023 10:49:00 -0700 Subject: [PATCH 055/153] spot_viewer: move robot strafe to 'qe' keys. Add mouse raycast object select. --- examples/spot_viewer.py | 46 +++++++++++++++++++++++------------------ 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 4552b89af6..338948995c 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -215,21 +215,8 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: key.W: False, key.X: False, key.Z: False, - } - - # set up our movement key bindings map - key = Application.KeyEvent.Key - self.key_to_action = { - key.UP: "look_up", - key.DOWN: "look_down", - key.LEFT: "turn_left", - key.RIGHT: "turn_right", - key.A: "move_left", - key.D: "move_right", - key.S: "move_backward", - key.W: "move_forward", - key.X: "move_down", - key.Z: "move_up", + key.Q: False, + key.E: False, } # Load a TrueTypeFont plugin and open the font file @@ -302,6 +289,9 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.camera_distance = 2.0 self.camera_angles = mn.Vector2() + # object selection and manipulation interface + self.selected_object = None + # configure our simulator self.cfg: Optional[habitat_sim.simulator.Configuration] = None self.sim: Optional[habitat_sim.simulator.Simulator] = None @@ -614,9 +604,9 @@ def move_and_look(self, repetitions: int) -> None: if abs(self.spot_forward) < min_val: self.spot_forward = 0 - if press[key.LEFT] and not press[key.RIGHT]: + if press[key.Q] and not press[key.E]: self.spot_lateral = max(min_val, self.spot_lateral + inc) - elif press[key.RIGHT] and not press[key.LEFT]: + elif press[key.E] and not press[key.Q]: self.spot_lateral = min(-min_val, self.spot_lateral - inc) else: self.spot_lateral /= 2.0 @@ -771,8 +761,24 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: Handles `Application.MouseEvent`. When in GRAB mode, click on objects to drag their position. (right-click for fixed constraints) """ - # button = Application.MouseEvent.Button - # physics_enabled = self.sim.get_physics_simulation_library() + button = Application.MouseEvent.Button + physics_enabled = self.sim.get_physics_simulation_library() + mod = Application.InputEvent.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + + # select an object with Shift+RIGHT-click + if physics_enabled and event.button == button.RIGHT and shift_pressed: + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(event.position)) + mouse_cast_results = self.sim.cast_ray(ray=ray) + hit_id = mouse_cast_results.hits[0].object_id + rom = self.sim.get_rigid_object_manager() + if hit_id == -1: + print("This is the stage.") + elif rom.get_library_has_id(hit_id): + ro = rom.get_object_by_id(hit_id) + self.selected_object = ro + print(f"Rigid Object: {ro.handle}") self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() @@ -888,7 +894,7 @@ def print_help_text(self) -> None: Agent Controls: 'wasd': Move Spot's body forward/backward and rotate left/right. - arrow keys: Move Spot's body forward/backwards and strafe left/right. + 'qe': Move Spot's body in strafe left/right. Utilities: 'r': Reset the simulator with the most recently loaded scene. From a73edab9dc3de8d8d7ceef005d46d6a2daa1a9d3 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 1 May 2023 13:40:45 -0700 Subject: [PATCH 056/153] spot_viewer: Add object modification interface features. --- examples/spot_viewer.py | 152 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 149 insertions(+), 3 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 338948995c..6872ff5b4c 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -291,6 +291,12 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # object selection and manipulation interface self.selected_object = None + # cache modified states of any objects moved by the interface. + self.modified_objects_buffer: Dict[ + habitat_sim.physics.ManagedRigidObject, mn.Matrix4 + ] = {} + self.translation_speed = 0.05 + self.rotation_speed = 0.1 # configure our simulator self.cfg: Optional[habitat_sim.simulator.Configuration] = None @@ -368,6 +374,41 @@ def debug_draw(self): self.sim.physics_debug_draw(proj_mat) if self.contact_debug_draw: self.draw_contact_debug() + if self.selected_object is not None: + aabb = self.selected_object.collision_shape_aabb + dblr = self.sim.get_debug_line_render() + dblr.push_transform(self.selected_object.transformation) + dblr.draw_box(aabb.min, aabb.max, mn.Color4.magenta()) + dblr.pop_transform() + ot = self.selected_object.translation + # draw global coordinate axis + dblr.draw_transformed_line( + ot - mn.Vector3.x_axis(), ot + mn.Vector3.x_axis(), mn.Color4.red() + ) + dblr.draw_transformed_line( + ot - mn.Vector3.y_axis(), ot + mn.Vector3.y_axis(), mn.Color4.green() + ) + dblr.draw_transformed_line( + ot - mn.Vector3.z_axis(), ot + mn.Vector3.z_axis(), mn.Color4.blue() + ) + dblr.draw_circle( + ot + mn.Vector3.x_axis() * 0.95, + radius=0.05, + color=mn.Color4.red(), + normal=mn.Vector3.x_axis(), + ) + dblr.draw_circle( + ot + mn.Vector3.y_axis() * 0.95, + radius=0.05, + color=mn.Color4.green(), + normal=mn.Vector3.y_axis(), + ) + dblr.draw_circle( + ot + mn.Vector3.z_axis() * 0.95, + radius=0.05, + color=mn.Color4.blue(), + normal=mn.Vector3.z_axis(), + ) def draw_event( self, @@ -595,9 +636,9 @@ def move_and_look(self, repetitions: int) -> None: inc = 0.02 min_val = 0.1 - if (press[key.UP] or press[key.W]) and not (press[key.DOWN] or press[key.S]): + if press[key.W] and not press[key.S]: self.spot_forward = max(min_val, self.spot_forward + inc) - elif not (press[key.UP] or press[key.W]) and (press[key.DOWN] or press[key.S]): + elif press[key.S] and not press[key.W]: self.spot_forward = min(-min_val, self.spot_forward - inc) else: self.spot_forward /= 2.0 @@ -636,6 +677,29 @@ def invert_gravity(self) -> None: gravity: mn.Vector3 = self.sim.get_gravity() * -1 self.sim.set_gravity(gravity) + def move_selected_object( + self, + translation: Optional[mn.Vector3] = None, + rotation: Optional[mn.Quaternion] = None, + ): + """ + Move the selected object with a given modification and save the resulting state to the buffer. + """ + modify_buffer = translation is not None or rotation is not None + if self.selected_object is not None and modify_buffer: + self.selected_object.motion_type = habitat_sim.physics.MotionType.KINEMATIC + if translation is not None: + self.selected_object.translation = ( + self.selected_object.translation + translation + ) + if rotation is not None: + self.selected_object.rotation = rotation * self.selected_object.rotation + self.selected_object.motion_type = habitat_sim.physics.MotionType.STATIC + self.navmesh_config_and_recompute() + self.modified_objects_buffer[ + self.selected_object + ] = self.selected_object.transformation + def key_press_event(self, event: Application.KeyEvent) -> None: """ Handles `Application.KeyEvent` on a key press by performing the corresponding functions. @@ -650,6 +714,15 @@ def key_press_event(self, event: Application.KeyEvent) -> None: alt_pressed = bool(event.modifiers & mod.ALT) # warning: ctrl doesn't always pass through with other key-presses + obj_translation_speed = ( + self.translation_speed + if not shift_pressed + else self.translation_speed * 2.0 + ) + obj_rotation_speed = ( + self.rotation_speed if not shift_pressed else self.rotation_speed * 2.0 + ) + if key == pressed.ESC: event.accepted = True self.exit_event(Application.ExitEvent) @@ -694,6 +767,68 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.contact_debug_draw = True # TODO: add a nice log message with concise contact pair naming. + elif key == pressed.LEFT: + if alt_pressed: + self.move_selected_object( + rotation=mn.Quaternion.rotation( + mn.Rad(obj_rotation_speed), mn.Vector3.y_axis() + ) + ) + else: + self.move_selected_object( + translation=mn.Vector3.x_axis() * obj_translation_speed + ) + elif key == pressed.RIGHT: + if alt_pressed: + self.move_selected_object( + rotation=mn.Quaternion.rotation( + -mn.Rad(obj_rotation_speed), mn.Vector3.y_axis() + ) + ) + else: + self.move_selected_object( + translation=-mn.Vector3.x_axis() * obj_translation_speed + ) + elif key == pressed.UP: + if alt_pressed: + self.move_selected_object( + translation=mn.Vector3.y_axis() * obj_translation_speed + ) + else: + self.move_selected_object( + translation=mn.Vector3.z_axis() * obj_translation_speed + ) + elif key == pressed.DOWN: + if alt_pressed: + self.move_selected_object( + translation=-mn.Vector3.y_axis() * obj_translation_speed + ) + else: + self.move_selected_object( + translation=-mn.Vector3.z_axis() * obj_translation_speed + ) + elif key == pressed.BACKSPACE: + if self.selected_object is not None: + print(f"Removed {self.selected_object.handle}") + self.sim.get_rigid_object_manager().remove_object_by_handle( + self.selected_object.handle + ) + self.selected_object = None + self.navmesh_config_and_recompute() + + elif key == pressed.I: + # dump the modified object states buffer to JSON. + # print(f"Writing modified_objects_buffer to 'scene_mod_buffer.json': {self.modified_objects_buffer}") + # with open("scene_mod_buffer.json", "w") as f: + # f.write(json.dumps(self.modified_objects_buffer, indent=2)) + aom = self.sim.get_articulated_object_manager() + aom.remove_all_objects() + self.sim.save_current_scene_config() + print( + "Saved modified scene instance JSON to original location. Look for ' (copy:0000)' or similar." + ) + exit() + elif key == pressed.T: pass @@ -768,6 +903,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: # select an object with Shift+RIGHT-click if physics_enabled and event.button == button.RIGHT and shift_pressed: + self.selected_object = None render_camera = self.render_camera.render_camera ray = render_camera.unproject(self.get_mouse_position(event.position)) mouse_cast_results = self.sim.cast_ray(ray=ray) @@ -892,10 +1028,20 @@ def print_help_text(self) -> None: esc: Exit the application. 'h': Display this help message. - Agent Controls: + Spot Controls: 'wasd': Move Spot's body forward/backward and rotate left/right. 'qe': Move Spot's body in strafe left/right. + Scene Object Modification UI: + 'SHIFT+right-click': Select an object to modify. + - With an object selected: + - LEFT/RIGHT arrow keys: move the object along global X axis. + (+ALT): rotate the object around Y axis + - UP/DOWN arrow keys: move the object along global Z axis. + (+ALT): move the object up/down (global Y axis) + - BACKSPACE: delete the selected object + 'i': save the current, modified, scene_instance file and close the viewer. + Utilities: 'r': Reset the simulator with the most recently loaded scene. 'n': Show/hide NavMesh wireframe. From 6fbec1a754591adfd50045c902cd16430aeaa36f Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 1 May 2023 14:36:04 -0700 Subject: [PATCH 057/153] add clutter removal with 'c' + tracking in removed_clutter.txt --- examples/spot_viewer.py | 31 ++++++++++++------------------- 1 file changed, 12 insertions(+), 19 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 6872ff5b4c..9af3ad4eeb 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -295,6 +295,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.modified_objects_buffer: Dict[ habitat_sim.physics.ManagedRigidObject, mn.Matrix4 ] = {} + self.removed_clutter = [] self.translation_speed = 0.05 self.rotation_speed = 0.1 @@ -752,21 +753,6 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.debug_bullet_draw = not self.debug_bullet_draw logger.info(f"Command: toggle Bullet debug draw: {self.debug_bullet_draw}") - elif key == pressed.C: - if shift_pressed: - self.contact_debug_draw = not self.contact_debug_draw - logger.info( - f"Command: toggle contact debug draw: {self.contact_debug_draw}" - ) - else: - # perform a discrete collision detection pass and enable contact debug drawing to visualize the results - logger.info( - "Command: perform discrete collision detection and visualize active contacts." - ) - self.sim.perform_discrete_collision_detection() - self.contact_debug_draw = True - # TODO: add a nice log message with concise contact pair naming. - elif key == pressed.LEFT: if alt_pressed: self.move_selected_object( @@ -807,8 +793,11 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.move_selected_object( translation=-mn.Vector3.z_axis() * obj_translation_speed ) - elif key == pressed.BACKSPACE: + elif key == pressed.BACKSPACE or key == pressed.C: if self.selected_object is not None: + if key == pressed.C: + obj_name = self.selected_object.handle.split("/")[-1].split("_:")[0] + self.removed_clutter.append(obj_name) print(f"Removed {self.selected_object.handle}") self.sim.get_rigid_object_manager().remove_object_by_handle( self.selected_object.handle @@ -827,6 +816,11 @@ def key_press_event(self, event: Application.KeyEvent) -> None: print( "Saved modified scene instance JSON to original location. Look for ' (copy:0000)' or similar." ) + # de-duplicate and save clutter list + self.removed_clutter = list(dict.fromkeys(self.removed_clutter)) + with open("removed_clutter.txt", "a") as f: + for obj_name in self.removed_clutter: + f.write(obj_name + "\n") exit() elif key == pressed.T: @@ -1040,7 +1034,8 @@ def print_help_text(self) -> None: - UP/DOWN arrow keys: move the object along global Z axis. (+ALT): move the object up/down (global Y axis) - BACKSPACE: delete the selected object - 'i': save the current, modified, scene_instance file and close the viewer. + - 'c': delete the selected object and record it as clutter. + 'i': save the current, modified, scene_instance file and close the viewer. Also save removed_clutter.txt containing object names of all removed clutter objects. Utilities: 'r': Reset the simulator with the most recently loaded scene. @@ -1048,8 +1043,6 @@ def print_help_text(self) -> None: (+SHIFT) Recompute NavMesh with Spot settings (already done). (+ALT) Re-sample Spot's position from the NavMesh. ',': Render a Bullet collision shape debug wireframe overlay (white=active, green=sleeping, blue=wants sleeping, red=can't sleep). - 'c': Run a discrete collision detection pass and render a debug wireframe overlay showing active contact points and normals (yellow=fixed length normals, red=collision distances). - (+SHIFT) Toggle the contact point debug render overlay on/off. Object Interactions: SPACE: Toggle physics simulation on/off. From 11fe157c50275ce823d2e1da3564edfcbb89bd02 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 1 May 2023 16:13:59 -0700 Subject: [PATCH 058/153] obj_viewer compares can with cylinder --- examples/obj_viewer.py | 21 +++++++++++++++++++-- tools/collision_shape_automation.py | 2 +- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/examples/obj_viewer.py b/examples/obj_viewer.py index 0502cae27f..ecb35b2039 100644 --- a/examples/obj_viewer.py +++ b/examples/obj_viewer.py @@ -547,6 +547,9 @@ def reconfigure_sim(self) -> None: for composite_file in sim_settings["composite_files"]: self.replay_renderer.preload_file(composite_file) + otm = self.sim.metadata_mediator.object_template_manager + otm.load_configs("data/objects/ycb/configs/") + Timer.start() self.step = -1 @@ -824,7 +827,7 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: self.redraw() event.accepted = True - def construct_cylinder_object( + def construct_cylinder_object2( self, cyl_radius: float = 0.04, cyl_height: float = 0.15 ): constructed_cyl_temp_name = "scaled_cyl_template" @@ -835,6 +838,13 @@ def construct_cylinder_object( otm.register_template(cyl_temp, constructed_cyl_temp_name) return constructed_cyl_temp_name + def construct_cylinder_object( + self, cyl_radius: float = 0.04, cyl_height: float = 0.15 + ): + otm = self.sim.metadata_mediator.object_template_manager + cyl_temp_handle = otm.get_template_handles("chef")[0] + return cyl_temp_handle + def mouse_press_event(self, event: Application.MouseEvent) -> None: """ Handles `Application.MouseEvent`. When in GRAB mode, click on @@ -934,7 +944,14 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: and self.mouse_cast_results.has_hits() and event.button == button.RIGHT ): - constructed_cyl_obj_handle = self.construct_cylinder_object() + constructed_cyl_obj_handle = None + import random + + r = random.randint(0, 1) + if r == 0: + constructed_cyl_obj_handle = self.construct_cylinder_object() + else: + constructed_cyl_obj_handle = self.construct_cylinder_object2() # try to place an object if ( mn.math.dot( diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index f6e1101f7a..735ca3115f 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -1192,7 +1192,7 @@ def compute_receptacle_stability( use_gt: bool = False, cyl_radius: float = 0.04, cyl_height: float = 0.15, - accepted_height_error: float = 0.05, + accepted_height_error: float = 0.1, ): """ Try to place a dynamic cylinder on the receptacle points. Record snap error and physical stability. From a49eec3bb45dad82824129b0763d5ce44559c3f9 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 1 May 2023 16:43:51 -0700 Subject: [PATCH 059/153] UI improvements for viewer app --- examples/viewer.py | 81 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 80 insertions(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index 97b9fe1231..7c9b1388fb 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -19,6 +19,7 @@ import magnum as mn import numpy as np from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler +from habitat.sims.habitat_simulator.sim_utilities import get_bb_corners from magnum import shaders, text from magnum.platform.glfw import Application @@ -457,6 +458,7 @@ def export_filtered_recs(self, filepath: Optional[str] = None) -> None: os.makedirs(os.path.dirname(filepath), exist_ok=True) with open(filepath, "w") as f: f.write(json.dumps(self.rec_filter_data, indent=2)) + print(f"Exported filter annotations to {filepath}.") def load_filtered_recs(self, filepath: Optional[str] = None) -> None: """ @@ -478,6 +480,7 @@ def load_filtered_recs(self, filepath: Optional[str] = None) -> None: assert "access_filtered" in self.rec_filter_data assert "stability_filtered" in self.rec_filter_data assert "height_filtered" in self.rec_filter_data + print(f"Loaded filter annotations from {filepath}") def load_receptacles(self): """ @@ -576,6 +579,7 @@ def debug_draw(self): """ Additional draw commands to be called during draw_event. """ + rom = self.sim.get_rigid_object_manager() if self.debug_bullet_draw: render_cam = self.render_camera.render_camera proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) @@ -680,7 +684,82 @@ def debug_draw(self): ): rec_color = mn.Color4.blue() - receptacle.debug_draw(self.sim, color=rec_color) + rec_obj = rom.get_object_by_handle(receptacle.parent_object_handle) + key_points = [r_trans.translation] + key_points.extend(get_bb_corners(rec_obj.root_scene_node.cumulative_bb)) + + in_view = False + for ix, key_point in enumerate(key_points): + r_pos = key_point + if ix > 0: + r_pos = rec_obj.transformation.transform_point(key_point) + c_to_r = r_pos - c_pos + # only display receptacles within 4 meters centered in view + if ( + c_to_r.length() < 4 + and mn.math.dot((c_to_r).normalized(), c_forward) > 0.7 + ): + in_view = True + break + if in_view: + # handle coloring + rec_color = None + if self.selected_rec == receptacle: + # white + rec_color = mn.Color4.cyan() + elif ( + self.cpo_initialized + and self.rec_color_mode != RecColorMode.DEFAULT + ): + if self.rec_color_mode == RecColorMode.GT_STABILITY: + rec_color = rg_lerp.at( + rec_dat["shape_id_results"]["gt"]["stability_results"][ + "success_ratio" + ] + ) + elif self.rec_color_mode == RecColorMode.GT_ACCESS: + rec_color = rg_lerp.at( + rec_dat["shape_id_results"]["gt"]["access_results"][ + "receptacle_access_score" + ] + ) + elif self.rec_color_mode == RecColorMode.PR_STABILITY: + rec_color = rg_lerp.at( + rec_dat["shape_id_results"]["pr0"]["stability_results"][ + "success_ratio" + ] + ) + elif self.rec_color_mode == RecColorMode.PR_ACCESS: + rec_color = rg_lerp.at( + rec_dat["shape_id_results"]["pr0"]["access_results"][ + "receptacle_access_score" + ] + ) + elif self.rec_color_mode == RecColorMode.FILTERING: + if rec_unique_name in self.rec_filter_data["active"]: + rec_color = mn.Color4.green() + elif ( + rec_unique_name + in self.rec_filter_data["manually_filtered"] + ): + rec_color = mn.Color4.yellow() + elif ( + rec_unique_name + in self.rec_filter_data["access_filtered"] + ): + rec_color = mn.Color4.red() + elif ( + rec_unique_name + in self.rec_filter_data["stability_filtered"] + ): + rec_color = mn.Color4.magenta() + elif ( + rec_unique_name + in self.rec_filter_data["height_filtered"] + ): + rec_color = mn.Color4.blue() + + receptacle.debug_draw(self.sim, color=rec_color) # mouse raycast circle white = mn.Color4(mn.Vector3(1.0), 1.0) From da9a72ebf5b72851cea256ef469817d33617cb65 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 16 May 2023 11:09:21 -0700 Subject: [PATCH 060/153] useability improvements for spot_viewer.py --- examples/spot_viewer.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 9af3ad4eeb..d602005e2e 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -298,6 +298,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.removed_clutter = [] self.translation_speed = 0.05 self.rotation_speed = 0.1 + self.navmesh_dirty = False # configure our simulator self.cfg: Optional[habitat_sim.simulator.Configuration] = None @@ -441,6 +442,9 @@ def draw_event( simulation_call() if global_call is not None: global_call() + if self.navmesh_dirty: + self.navmesh_config_and_recompute() + self.navmesh_dirty = False # reset time_since_last_simulation, accounting for potential overflow self.time_since_last_simulation = math.fmod( @@ -696,7 +700,7 @@ def move_selected_object( if rotation is not None: self.selected_object.rotation = rotation * self.selected_object.rotation self.selected_object.motion_type = habitat_sim.physics.MotionType.STATIC - self.navmesh_config_and_recompute() + self.navmesh_dirty = True self.modified_objects_buffer[ self.selected_object ] = self.selected_object.transformation @@ -812,7 +816,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # f.write(json.dumps(self.modified_objects_buffer, indent=2)) aom = self.sim.get_articulated_object_manager() aom.remove_all_objects() - self.sim.save_current_scene_config() + self.sim.save_current_scene_config(overwrite=True) print( "Saved modified scene instance JSON to original location. Look for ' (copy:0000)' or similar." ) From c285e2676c76c5eeeff12009fe2d62b3d6aed48e Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 16 May 2023 11:14:22 -0700 Subject: [PATCH 061/153] add ssd removal script for post-processing scene_instance.json files after exporting them from spot_viewer --- tools/remove_ssd_from_scene_instance.py | 83 +++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 tools/remove_ssd_from_scene_instance.py diff --git a/tools/remove_ssd_from_scene_instance.py b/tools/remove_ssd_from_scene_instance.py new file mode 100644 index 0000000000..0cdfcc4675 --- /dev/null +++ b/tools/remove_ssd_from_scene_instance.py @@ -0,0 +1,83 @@ +# 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 argparse +import json +import os +from typing import Callable, List + + +def file_is_scene_config(filepath: str) -> bool: + """ + Return whether or not the file is an scene_instance.json + """ + return filepath.endswith(".scene_instance.json") + + +def find_files(root_dir: str, discriminator: Callable[[str], bool]) -> List[str]: + """ + Recursively find all filepaths under a root directory satisfying a particular constraint as defined by a discriminator function. + + :param root_dir: The roor directory for the recursive search. + :param discriminator: The discriminator function which takes a filepath and returns a bool. + + :return: The list of all absolute filepaths found satisfying the discriminator. + """ + filepaths: List[str] = [] + + if not os.path.exists(root_dir): + print(" Directory does not exist: " + str(dir)) + return filepaths + + for entry in os.listdir(root_dir): + entry_path = os.path.join(root_dir, entry) + if os.path.isdir(entry_path): + sub_dir_filepaths = find_files(entry_path, discriminator) + filepaths.extend(sub_dir_filepaths) + # apply a user-provided discriminator function to cull filepaths + elif discriminator(entry_path): + filepaths.append(entry_path) + return filepaths + + +def remove_ssd_from_scene_instance_json(filepath: str): + """ + Strips any 'semantic_scene_instance' field from a scene_instance.json files and re-exports it. + """ + assert filepath.endswith(".scene_instance.json"), "Must be a scene instance JSON." + + file_is_modified = False + scene_conf = None + with open(filepath, "r") as f: + scene_conf = json.load(f) + if "semantic_scene_instance" in scene_conf: + scene_conf.pop("semantic_scene_instance") + file_is_modified = True + + # write the data as necessary + if file_is_modified and scene_conf is not None: + with open(filepath, "w") as f: + json.dump(scene_conf, f) + + +def main(): + parser = argparse.ArgumentParser( + description="Remove all 'semantic_scene_instance' fields from scene_instnace files in the dataset." + ) + parser.add_argument( + "--dataset-root-dir", + type=str, + help="path to HSSD SceneDataset root directory containing 'fphab-uncluttered.scene_dataset_config.json'.", + ) + args = parser.parse_args() + fp_root_dir = args.dataset_root_dir + config_root_dir = os.path.join(fp_root_dir, "scenes-uncluttered") + configs = find_files(config_root_dir, file_is_scene_config) + + for _ix, filepath in enumerate(configs): + remove_ssd_from_scene_instance_json(filepath) + + +if __name__ == "__main__": + main() From b07e6abc7c43b5f3017c3e6e8f290dd889c63621 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 19 May 2023 10:36:57 -0700 Subject: [PATCH 062/153] protect from crash when raycast hits nothing --- examples/spot_viewer.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index d602005e2e..f015a1ad0e 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -905,14 +905,15 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: render_camera = self.render_camera.render_camera ray = render_camera.unproject(self.get_mouse_position(event.position)) mouse_cast_results = self.sim.cast_ray(ray=ray) - hit_id = mouse_cast_results.hits[0].object_id - rom = self.sim.get_rigid_object_manager() - if hit_id == -1: - print("This is the stage.") - elif rom.get_library_has_id(hit_id): - ro = rom.get_object_by_id(hit_id) - self.selected_object = ro - print(f"Rigid Object: {ro.handle}") + if mouse_cast_results.has_hits(): + hit_id = mouse_cast_results.hits[0].object_id + rom = self.sim.get_rigid_object_manager() + if hit_id == -1: + print("This is the stage.") + elif rom.get_library_has_id(hit_id): + ro = rom.get_object_by_id(hit_id) + self.selected_object = ro + print(f"Rigid Object: {ro.handle}") self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() From b3790835b9b1bb86f54e08530956e5de8dbe585b Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 1 Jun 2023 14:48:54 -0700 Subject: [PATCH 063/153] viewer.py receptacle filter view no longer requires cpo --- examples/viewer.py | 43 +++++++++++++++++++------------------------ 1 file changed, 19 insertions(+), 24 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 7c9b1388fb..e84acce113 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -237,7 +237,7 @@ def __init__( self.cpo_initialized = False self.show_filtered = True self.rec_access_filter_threshold = 0.12 # empirically chosen - self.rec_color_mode = RecColorMode.DEFAULT + self.rec_color_mode = RecColorMode.FILTERING # map receptacle to parent objects self.rec_to_poh: Dict[hab_receptacle.Receptacle, str] = {} # contains filtering metadata and classification of meshes filtered automatically and manually @@ -707,6 +707,24 @@ def debug_draw(self): if self.selected_rec == receptacle: # white rec_color = mn.Color4.cyan() + elif ( + self.rec_filter_data is not None + ) and self.rec_color_mode == RecColorMode.FILTERING: + if rec_unique_name in self.rec_filter_data["active"]: + rec_color = mn.Color4.green() + elif ( + rec_unique_name in self.rec_filter_data["manually_filtered"] + ): + rec_color = mn.Color4.yellow() + elif rec_unique_name in self.rec_filter_data["access_filtered"]: + rec_color = mn.Color4.red() + elif ( + rec_unique_name + in self.rec_filter_data["stability_filtered"] + ): + rec_color = mn.Color4.magenta() + elif rec_unique_name in self.rec_filter_data["height_filtered"]: + rec_color = mn.Color4.blue() elif ( self.cpo_initialized and self.rec_color_mode != RecColorMode.DEFAULT @@ -735,29 +753,6 @@ def debug_draw(self): "receptacle_access_score" ] ) - elif self.rec_color_mode == RecColorMode.FILTERING: - if rec_unique_name in self.rec_filter_data["active"]: - rec_color = mn.Color4.green() - elif ( - rec_unique_name - in self.rec_filter_data["manually_filtered"] - ): - rec_color = mn.Color4.yellow() - elif ( - rec_unique_name - in self.rec_filter_data["access_filtered"] - ): - rec_color = mn.Color4.red() - elif ( - rec_unique_name - in self.rec_filter_data["stability_filtered"] - ): - rec_color = mn.Color4.magenta() - elif ( - rec_unique_name - in self.rec_filter_data["height_filtered"] - ): - rec_color = mn.Color4.blue() receptacle.debug_draw(self.sim, color=rec_color) From 664c54590cf03cbd19a1b219e3fe64543dba80bf Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 1 Jun 2023 14:57:10 -0700 Subject: [PATCH 064/153] additional changes to support navmesh refactor --- examples/obj_viewer.py | 3 +-- examples/spot_viewer.py | 7 ++----- tools/collision_shape_automation.py | 2 ++ 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/examples/obj_viewer.py b/examples/obj_viewer.py index ecb35b2039..0c241d8b41 100644 --- a/examples/obj_viewer.py +++ b/examples/obj_viewer.py @@ -1089,11 +1089,10 @@ def navmesh_config_and_recompute(self) -> None: self.navmesh_settings.set_defaults() self.navmesh_settings.agent_height = self.cfg.agents[self.agent_id].height self.navmesh_settings.agent_radius = self.cfg.agents[self.agent_id].radius - + self.navmesh_settings.include_static_objects = True self.sim.recompute_navmesh( self.sim.pathfinder, self.navmesh_settings, - include_static_objects=True, ) def exit_event(self, event: Application.ExitEvent): diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index f015a1ad0e..d76a12a1d9 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -972,11 +972,8 @@ def navmesh_config_and_recompute(self) -> None: self.navmesh_settings.set_defaults() self.navmesh_settings.agent_height = self.cfg.agents[self.agent_id].height self.navmesh_settings.agent_radius = 0.3 - self.sim.recompute_navmesh( - self.sim.pathfinder, - self.navmesh_settings, - include_static_objects=True, - ) + self.navmesh_settings.include_static_objects = True + self.sim.recompute_navmesh(self.sim.pathfinder, self.navmesh_settings) def exit_event(self, event: Application.ExitEvent): """ diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 735ca3115f..3bf99773fd 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -2212,6 +2212,7 @@ def get_objects_in_scene( sim_settings = default_sim_settings.copy() sim_settings["scene_dataset_config_file"] = dataset_path sim_settings["scene"] = scene_handle + sim_settings["default_agent_navmesh"] = False cfg = make_cfg(sim_settings) cfg.metadata_mediator = mm @@ -2404,6 +2405,7 @@ def main(): sim_settings["width"] = 720 sim_settings["height"] = 720 sim_settings["clear_color"] = mn.Color4.magenta() * 0.5 + sim_settings["default_agent_navmesh"] = False # use the CollisionProxyOptimizer to compute metrics for multiple objects cpo = CollisionProxyOptimizer(sim_settings, output_directory=args.output_dir) From 3e9f76f2e6120d9f2d91854100652151bbdd4f11 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 1 Jun 2023 15:16:05 -0700 Subject: [PATCH 065/153] remove navmesh recompute from cpo settings in viewer.py --- examples/viewer.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/viewer.py b/examples/viewer.py index e84acce113..7ceaec1345 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1895,6 +1895,9 @@ def run_cpo_for_obj(obj_handle): sim_settings["enable_hbao"] = args.hbao sim_settings["clear_color"] = mn.Color4.magenta() + # don't need auto-navmesh + sim_settings["default_agent_navmesh"] = False + mm = habitat_sim.metadata.MetadataMediator() mm.active_dataset = sim_settings["scene_dataset_config_file"] From e1a74bf0fa8568fba816b9d43d2d6ba4fee1dd2e Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 14 Jun 2023 12:54:47 -0700 Subject: [PATCH 066/153] minor utility updates for GUI --- examples/viewer.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index 7ceaec1345..b7c505db07 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -710,6 +710,8 @@ def debug_draw(self): elif ( self.rec_filter_data is not None ) and self.rec_color_mode == RecColorMode.FILTERING: + # blue indicates no filter data for the receptacle, it may be newer than the filter file. + rec_color = mn.Color4.blue() if rec_unique_name in self.rec_filter_data["active"]: rec_color = mn.Color4.green() elif ( @@ -755,7 +757,14 @@ def debug_draw(self): ) receptacle.debug_draw(self.sim, color=rec_color) - + if True: + dblr = self.sim.get_debug_line_render() + t_form = receptacle.get_global_transform(self.sim) + dblr.push_transform(t_form) + dblr.draw_transformed_line( + mn.Vector3(0), receptacle.up, mn.Color4.cyan() + ) + dblr.pop_transform() # mouse raycast circle white = mn.Color4(mn.Vector3(1.0), 1.0) if self.mouse_cast_results is not None and self.mouse_cast_results.has_hits(): From 9597e72b2788572fab572675a3dfbe25cc2f41b1 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 7 Aug 2023 14:46:09 -0700 Subject: [PATCH 067/153] add slick hit visualization to easier identify roof hits --- examples/spot_viewer.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index d76a12a1d9..0e1f0e2ae1 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -291,6 +291,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # object selection and manipulation interface self.selected_object = None + self.last_hit_details = None # cache modified states of any objects moved by the interface. self.modified_objects_buffer: Dict[ habitat_sim.physics.ManagedRigidObject, mn.Matrix4 @@ -376,6 +377,14 @@ def debug_draw(self): self.sim.physics_debug_draw(proj_mat) if self.contact_debug_draw: self.draw_contact_debug() + if self.last_hit_details is not None: + self.sim.get_debug_line_render().draw_circle( + translation=self.last_hit_details.point, + radius=0.02, + normal=self.last_hit_details.normal, + color=mn.Color4.yellow(), + num_segments=12, + ) if self.selected_object is not None: aabb = self.selected_object.collision_shape_aabb dblr = self.sim.get_debug_line_render() @@ -906,6 +915,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: ray = render_camera.unproject(self.get_mouse_position(event.position)) mouse_cast_results = self.sim.cast_ray(ray=ray) if mouse_cast_results.has_hits(): + self.last_hit_details = mouse_cast_results.hits[0] hit_id = mouse_cast_results.hits[0].object_id rom = self.sim.get_rigid_object_manager() if hit_id == -1: From ad1b42324aa72789d3b3865400582d6e86387939 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 6 Sep 2023 16:32:38 -0700 Subject: [PATCH 068/153] update message --- examples/spot_viewer.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 0e1f0e2ae1..faa1baf4d9 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -826,9 +826,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: aom = self.sim.get_articulated_object_manager() aom.remove_all_objects() self.sim.save_current_scene_config(overwrite=True) - print( - "Saved modified scene instance JSON to original location. Look for ' (copy:0000)' or similar." - ) + print("Saved modified scene instance JSON to original location.") # de-duplicate and save clutter list self.removed_clutter = list(dict.fromkeys(self.removed_clutter)) with open("removed_clutter.txt", "a") as f: From 53794b912cde20450f503c054c8de5c9871f8711 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 2 Oct 2023 14:11:45 -0700 Subject: [PATCH 069/153] refactor to support new requirement in robot API init --- examples/spot_viewer.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index faa1baf4d9..a47da82888 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -18,6 +18,7 @@ import numpy as np from magnum import shaders, text from magnum.platform.glfw import Application +from omegaconf import DictConfig import habitat_sim from habitat_sim import ReplayRenderer, ReplayRendererConfiguration @@ -610,7 +611,8 @@ def reconfigure_sim(self) -> None: # add the robot to the world via the wrapper robot_path = "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" - self.spot = spot_robot.SpotRobot(robot_path, self.sim, fixed_base=True) + agent_config = DictConfig({"articulated_agent_urdf": robot_path}) + self.spot = spot_robot.SpotRobot(agent_config, self.sim, fixed_base=True) self.spot.reconfigure() self.spot.update() self.spot_action = ExtractedBaseVelNonCylinderAction(self.sim, self.spot) From f8ebc1ebfcf688fcd7597ec6a8efcf012f870f0d Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 2 Oct 2023 15:30:46 -0700 Subject: [PATCH 070/153] add automated outdoor object removal --- examples/spot_viewer.py | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index a47da82888..f2948b6689 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -301,6 +301,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.translation_speed = 0.05 self.rotation_speed = 0.1 self.navmesh_dirty = False + self.removed_objects_debug_frames = [] # configure our simulator self.cfg: Optional[habitat_sim.simulator.Configuration] = None @@ -321,6 +322,44 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: logger.setLevel("INFO") self.print_help_text() + def draw_removed_objects_debug_frames(self): + """ + Draw debug frames for all the recently removed objects. + """ + for trans, aabb in self.removed_objects_debug_frames: + dblr = self.sim.get_debug_line_render() + dblr.push_transform(trans) + dblr.draw_box(aabb.min, aabb.max, mn.Color4.red()) + dblr.pop_transform() + + def remove_outdoor_objects(self): + """ + Check all object instance and remove those which are marked outdoors. + """ + self.removed_objects_debug_frames = [] + rom = self.sim.get_rigid_object_manager() + for obj in rom.get_objects_by_handle_substring().values(): + if self.obj_is_outdoor(obj): + self.removed_objects_debug_frames.append( + (obj.transformation, obj.root_scene_node.cumulative_bb) + ) + rom.remove_object_by_id(obj.object_id) + + def obj_is_outdoor(self, obj): + """ + Check if an object is outdoors or not by raycasting upwards. + """ + up = mn.Vector3(0, 1.0, 0) + ray_results = self.sim.cast_ray(habitat_sim.geo.Ray(obj.translation, up)) + if ray_results.has_hits(): + for hit in ray_results.hits: + if hit.object_id == obj.object_id: + continue + return False + + # no hits, so outdoors + return True + def place_spot(self): if self.sim.pathfinder.is_loaded: valid_spot_point = None @@ -421,6 +460,7 @@ def debug_draw(self): color=mn.Color4.blue(), normal=mn.Vector3.z_axis(), ) + self.draw_removed_objects_debug_frames() def draw_event( self, @@ -837,6 +877,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: exit() elif key == pressed.T: + self.remove_outdoor_objects() pass elif key == pressed.V: From 571c9838c841426ef05b6338295cfe9a4d160671 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 2 Oct 2023 17:04:41 -0700 Subject: [PATCH 071/153] minor updates --- examples/spot_viewer.py | 5 +++-- examples/viewer.py | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index f2948b6689..32d4c23051 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -1196,13 +1196,13 @@ def next_frame() -> None: ) parser.add_argument( "--width", - default=800, + default=1080, type=int, help="Horizontal resolution of the window.", ) parser.add_argument( "--height", - default=600, + default=720, type=int, help="Vertical resolution of the window.", ) @@ -1228,6 +1228,7 @@ def next_frame() -> None: sim_settings["window_width"] = args.width sim_settings["window_height"] = args.height sim_settings["sensor_height"] = 0 + sim_settings["enable_hbao"] = True # start the application HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/examples/viewer.py b/examples/viewer.py index b7c505db07..56d15e8296 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1869,13 +1869,13 @@ def run_cpo_for_obj(obj_handle): ) parser.add_argument( "--width", - default=800, + default=1080, type=int, help="Horizontal resolution of the window.", ) parser.add_argument( "--height", - default=600, + default=720, type=int, help="Vertical resolution of the window.", ) From 1d927bad2205e2c22bd2a4411e334768adec3e87 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 3 Oct 2023 09:54:02 -0700 Subject: [PATCH 072/153] add automated filter file loading to viewer tool --- examples/viewer.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/examples/viewer.py b/examples/viewer.py index 56d15e8296..ef96d222b5 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -343,6 +343,30 @@ def modify_param_from_term(self): else: print("That attribute is unset, so I don't know the type.") + def load_scene_filter_file(self): + """ + Load the filter file for a scene from config. + """ + + scene_user_defined = self.sim.metadata_mediator.get_scene_user_defined( + self.sim.curr_scene_name + ) + if scene_user_defined is not None and scene_user_defined.has_value( + "scene_filter_file" + ): + scene_filter_file = scene_user_defined.get("scene_filter_file") + # construct the dataset level path for the filter data file + scene_filter_file = os.path.join( + os.path.dirname(mm.active_dataset), scene_filter_file + ) + print(f"scene_filter_file = {scene_filter_file}") + self.load_receptacles() + self.load_filtered_recs(scene_filter_file) + else: + print( + f"WARNING: No rec filter file configured for scene {self.sim.curr_scene_name}." + ) + def get_rec_instance_name(self, receptacle: hab_receptacle.Receptacle) -> str: """ Gets a unique string name for the Receptacle instance. From 18c8c127fafe15e8fadcaeabf7d1719417590812 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 3 Oct 2023 15:29:47 -0700 Subject: [PATCH 073/153] snap to largest island in viewer app --- examples/viewer.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index ef96d222b5..7201349008 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -18,6 +18,7 @@ import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle import magnum as mn import numpy as np +from habitat.datasets.rearrange.navmesh_utils import get_largest_island_index from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler from habitat.sims.habitat_simulator.sim_utilities import get_bb_corners from magnum import shaders, text @@ -362,6 +363,7 @@ def load_scene_filter_file(self): print(f"scene_filter_file = {scene_filter_file}") self.load_receptacles() self.load_filtered_recs(scene_filter_file) + self.rec_filter_path = scene_filter_file else: print( f"WARNING: No rec filter file configured for scene {self.sim.curr_scene_name}." @@ -1189,8 +1191,16 @@ def key_press_event(self, event: Application.KeyEvent) -> None: logger.info("Command: resample agent state from navmesh") if self.sim.pathfinder.is_loaded: new_agent_state = habitat_sim.AgentState() + largest_island_ix = get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) + print(f"Largest indoor island index = {largest_island_ix}") new_agent_state.position = ( - self.sim.pathfinder.get_random_navigable_point() + self.sim.pathfinder.get_random_navigable_point( + island_index=largest_island_ix + ) ) new_agent_state.rotation = quat_from_angle_axis( self.sim.random.uniform_float(0, 2.0 * np.pi), From 7570a2c007170d9ec75432d8a33d4de85d67fb83 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 17 Nov 2023 18:56:50 -0800 Subject: [PATCH 074/153] largest navmesh snapping and magnum API update --- examples/spot_viewer.py | 11 ++++++++++- examples/viewer.py | 4 ++-- tools/collision_shape_automation.py | 28 +++++++++++++++------------- 3 files changed, 27 insertions(+), 16 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 32d4c23051..bbfad96e5d 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -16,6 +16,7 @@ import habitat.articulated_agents.robots.spot_robot as spot_robot import magnum as mn import numpy as np +from habitat.datasets.rearrange.navmesh_utils import get_largest_island_index from magnum import shaders, text from magnum.platform.glfw import Application from omegaconf import DictConfig @@ -362,11 +363,19 @@ def obj_is_outdoor(self, obj): def place_spot(self): if self.sim.pathfinder.is_loaded: + largest_island_ix = get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) + print(f"Largest indoor island index = {largest_island_ix}") valid_spot_point = None max_attempts = 1000 attempt = 0 while valid_spot_point is None and attempt < max_attempts: - spot_point = self.sim.pathfinder.get_random_navigable_point() + spot_point = self.sim.pathfinder.get_random_navigable_point( + island_index=largest_island_ix + ) if self.sim.pathfinder.distance_to_closest_obstacle(spot_point) >= 0.25: valid_spot_point = spot_point attempt += 1 diff --git a/examples/viewer.py b/examples/viewer.py index 7201349008..f63b033cf9 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1825,11 +1825,11 @@ def run_cpo_for_obj(obj_handle): _cpo.compute_receptacle_access_metrics(obj_handle, use_gt=False) # run CPO initialization multi-threaded to unblock viewer initialization and use - import threading threads = [] for obj_handle in objects_in_scene: - threads.append(threading.Thread(target=run_cpo_for_obj, args=(obj_handle,))) + run_cpo_for_obj(obj_handle) + # threads.append(threading.Thread(target=run_cpo_for_obj, args=(obj_handle,))) for thread in threads: thread.start() diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index 3bf99773fd..df6cd3ecfc 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -157,7 +157,7 @@ def print_dict_structure(input_dict: Dict[Any, Any], whitespace: str = "") -> No if whitespace == "": print("-----------------------------------") print("Print Dict Structure Results:") - for key in input_dict.keys(): + for key in input_dict: if isinstance(input_dict[key], Dict): print(whitespace + f"{key}:-") print_dict_structure( @@ -1041,7 +1041,7 @@ def compute_receptacle_access_metrics( ) # collect hemisphere raycast samples for all receptacle sample points - for receptacle_name in obj_rec_data.keys(): + for receptacle_name in obj_rec_data: sample_point_ray_results: List[ List[habitat_sim.physics.RaycastResults] ] = [] @@ -1251,7 +1251,7 @@ def compute_receptacle_stability( # evaluation the sample points for each receptacle rec_data = self.gt_data[obj_handle]["receptacles"] - for rec_name in rec_data.keys(): + for rec_name in rec_data: sample_points = rec_data[rec_name]["sample_points"] failed_snap = 0 @@ -1294,8 +1294,10 @@ def compute_receptacle_stability( ).length() # NOTE: negative quaternion represents the same rotation, but gets a different angle error so check both angular_displacement = min( - mn.math.angle(cyl_test_obj.rotation, identity_q), - mn.math.angle(-1 * cyl_test_obj.rotation, identity_q), + mn.math.half_angle(cyl_test_obj.rotation, identity_q), + mn.math.half_angle( + -1 * cyl_test_obj.rotation, identity_q + ), ) if ( angular_displacement > rotation_limit @@ -1642,7 +1644,7 @@ def compute_gt_errors(self, obj_handle: str) -> None: "gt" in self.gt_data[obj_handle]["raycasts"] ), "Must have a ground truth to compare against. Should be generated in `setup_obj_gt(obj_handle)`." - for shape_id in self.gt_data[obj_handle]["raycasts"].keys(): + for shape_id in self.gt_data[obj_handle]["raycasts"]: self.setup_shape_test_results_cache(obj_handle, shape_id) if ( shape_id != "gt" @@ -1905,7 +1907,7 @@ def optimize_object_col_shape( settings_key = method + "_settings" best_shape_score = pr0_shape_score shape_scores = {} - for shape_id in self.gt_data[obj_h][settings_key].keys(): + for shape_id in self.gt_data[obj_h][settings_key]: shape_score = self.compute_shape_score(obj_h, shape_id) shape_scores[shape_id] = shape_score # we only want significantly better shapes (10% or 0.1 score better threshold) @@ -1949,7 +1951,7 @@ def cache_global_results(self) -> None: Do this after an object's computation is done (compute_gt_errors) before cleaning the gt data. """ - for obj_handle in self.gt_data.keys(): + for obj_handle in self.gt_data: # populate the high-level sub-cache definitions if obj_handle not in self.results: self.results[obj_handle] = { @@ -2046,9 +2048,9 @@ def save_results_to_csv(self, filename: str) -> None: active_shape_metrics = [] for _obj_handle, obj_results in self.results.items(): for _shape_id, shape_results in obj_results["shape_metrics"].items(): - for metric in shape_results.keys(): + for metric in shape_results: if metric == "col_grid": - for subdiv in shape_results["col_grid"].keys(): + for subdiv in shape_results["col_grid"]: if subdiv not in active_subdivs: active_subdivs.append(subdiv) else: @@ -2099,7 +2101,7 @@ def save_results_to_csv(self, filename: str) -> None: for _obj_handle, obj_results in self.results.items(): for _rec_name, rec_results in obj_results["receptacle_metrics"].items(): for _shape_id, shape_results in rec_results.items(): - for metric in shape_results.keys(): + for metric in shape_results: if metric not in active_rec_metrics: active_rec_metrics.append(metric) @@ -2286,7 +2288,7 @@ def correct_object_orientations( :param obj_orientations: A dict mapping object names (abridged, not handles) to Tuple of (up,front) orientation vectors. """ obj_handle_to_orientation = {} - for obj_name in obj_orientations.keys(): + for obj_name in obj_orientations: for obj_handle in obj_handles: if obj_name in obj_handle: obj_handle_to_orientation[obj_handle] = obj_orientations[obj_name] @@ -2559,7 +2561,7 @@ def main(): # intercept optimization to instead export a txt file with model ids for import into the model categorizer tool with open(args.export_fp_model_ids, "w") as f: aggregated_object_ids = [] - for _, scene_objects in scene_object_handles.items(): + for scene_objects in scene_object_handles.values(): rec_obj_in_scene = [ scene_objects[i] for i in range(len(scene_objects)) From 008707be777a08770768f6e20d2e7a793d4f598f Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 9 Feb 2024 08:59:41 -0800 Subject: [PATCH 075/153] object getter util --- examples/viewer.py | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index f63b033cf9..fc527bf6c1 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -10,7 +10,7 @@ import sys import time from enum import Enum -from typing import Any, Callable, Dict, List, Optional, Tuple +from typing import Any, Callable, Dict, List, Optional, Tuple, Union flags = sys.getdlopenflags() sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) @@ -508,6 +508,23 @@ def load_filtered_recs(self, filepath: Optional[str] = None) -> None: assert "height_filtered" in self.rec_filter_data print(f"Loaded filter annotations from {filepath}") + def get_object_by_handle( + self, handle: str + ) -> Union[ + habitat_sim.physics.ManagedRigidObject, + habitat_sim.physics.ManagedArticulatedObject, + ]: + """ + Get either a rigid or articulated object from the handle. If none is found, returns None. + """ + rom = self.sim.get_rigid_object_manager() + if rom.get_library_has_handle(handle): + return rom.get_object_by_handle(handle) + aom = self.sim.get_articulated_object_manager() + if aom.get_library_has_handle(handle): + return aom.get_object_by_handle(handle) + return None + def load_receptacles(self): """ Load all receptacle data and setup helper datastructures. @@ -520,11 +537,9 @@ def load_receptacles(self): ] for receptacle in self.receptacles: if receptacle not in self.rec_to_poh: - po_handle = ( - self.sim.get_rigid_object_manager() - .get_object_by_handle(receptacle.parent_object_handle) - .creation_attributes.handle - ) + po_handle = self.get_object_by_handle( + receptacle.parent_object_handle + ).creation_attributes.handle self.rec_to_poh[receptacle] = po_handle def add_col_proxy_object( From 62bbc972c252e4938651da2927b177f92092fe1e Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 9 Feb 2024 11:17:07 -0800 Subject: [PATCH 076/153] fixes for aos --- examples/spot_viewer.py | 3 ++- examples/viewer.py | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index bbfad96e5d..228f3efd16 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -875,7 +875,8 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # with open("scene_mod_buffer.json", "w") as f: # f.write(json.dumps(self.modified_objects_buffer, indent=2)) aom = self.sim.get_articulated_object_manager() - aom.remove_all_objects() + aom.remove_object_by_handle(self.spot.sim_obj.handle) + # aom.remove_all_objects() self.sim.save_current_scene_config(overwrite=True) print("Saved modified scene instance JSON to original location.") # de-duplicate and save clutter list diff --git a/examples/viewer.py b/examples/viewer.py index fc527bf6c1..b995921e03 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -620,7 +620,7 @@ def debug_draw(self): """ Additional draw commands to be called during draw_event. """ - rom = self.sim.get_rigid_object_manager() + self.sim.get_rigid_object_manager() if self.debug_bullet_draw: render_cam = self.render_camera.render_camera proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) @@ -725,7 +725,7 @@ def debug_draw(self): ): rec_color = mn.Color4.blue() - rec_obj = rom.get_object_by_handle(receptacle.parent_object_handle) + rec_obj = self.get_object_by_handle(receptacle.parent_object_handle) key_points = [r_trans.translation] key_points.extend(get_bb_corners(rec_obj.root_scene_node.cumulative_bb)) From 545d73ecb3b9e9843492b9888add358cce41701c Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 9 Feb 2024 13:08:05 -0800 Subject: [PATCH 077/153] enable AO receptacle selection --- examples/viewer.py | 55 +++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 49 insertions(+), 6 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index b995921e03..52714196e2 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -272,6 +272,8 @@ def __init__( self.replay_renderer: Optional[ReplayRenderer] = None self.reconfigure_sim() self.debug_semantic_colors = {} + self.load_scene_filter_file() + self.ao_link_map = self.get_ao_link_id_map() # ----------------------------------------- # Clutter Generation Integration: @@ -508,6 +510,29 @@ def load_filtered_recs(self, filepath: Optional[str] = None) -> None: assert "height_filtered" in self.rec_filter_data print(f"Loaded filter annotations from {filepath}") + def get_ao_link_id_map(self) -> Dict[int, int]: + """ + Construct a map of ao_link object ids to their parent ao's object id. + NOTE: also maps ao's root object id to itself for ease of use. + + :param sim: The Simulator instance. + + :return: dictionary mapping ArticulatedLink object ids to their parent's object id. + """ + + aom = self.sim.get_articulated_object_manager() + ao_link_map: Dict[int, int] = {} + for ao in aom.get_objects_by_handle_substring().values(): + # add the ao itself for ease of use + ao_link_map[ao.object_id] = ao.object_id + # add the links + for link_id in ao.link_object_ids: + ao_link_map[link_id] = ao.object_id + + # print(f"ao_link_map = {ao_link_map}") + + return ao_link_map + def get_object_by_handle( self, handle: str ) -> Union[ @@ -525,6 +550,25 @@ def get_object_by_handle( return aom.get_object_by_handle(handle) return None + def get_object_by_id( + self, object_id: int + ) -> Union[ + habitat_sim.physics.ManagedRigidObject, + habitat_sim.physics.ManagedArticulatedObject, + ]: + """ + Get either a rigid or articulated object from the handle. If none is found, returns None. + """ + + rom = self.sim.get_rigid_object_manager() + if rom.get_library_has_id(object_id): + return rom.get_object_by_id(object_id) + aom = self.sim.get_articulated_object_manager() + if object_id in self.ao_link_map: + return aom.get_object_by_id(self.ao_link_map[object_id]) + + return None + def load_receptacles(self): """ Load all receptacle data and setup helper datastructures. @@ -1397,17 +1441,16 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: self.selected_object = None self.selected_rec = None hit_id = self.mouse_cast_results.hits[0].object_id - rom = self.sim.get_rigid_object_manager() # right click in look mode to print object information if hit_id == -1: print("This is the stage.") - elif rom.get_library_has_id(hit_id): - ro = rom.get_object_by_id(hit_id) - self.selected_object = ro - print(f"Rigid Object: {ro.handle}") + else: + obj = self.get_object_by_id(hit_id) + self.selected_object = obj + print(f"Object: {obj.handle}") if self.receptacles is not None: for rec in self.receptacles: - if rec.parent_object_handle == ro.handle: + if rec.parent_object_handle == obj.handle: print(f" - Receptacle: {rec.name}") if shift_pressed: self.selected_rec = self.get_closest_tri_receptacle( From 6fe2570a0ac7477b3fcc06cde3e9032b1ce58c83 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 23 Feb 2024 10:19:28 -0800 Subject: [PATCH 078/153] update to support selecting and transforming ArticulatedObjects --- examples/spot_viewer.py | 35 +++++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 228f3efd16..653eac0832 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -14,6 +14,7 @@ sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) import habitat.articulated_agents.robots.spot_robot as spot_robot +import habitat.sims.habitat_simulator.sim_utilities as sutils import magnum as mn import numpy as np from habitat.datasets.rearrange.navmesh_utils import get_largest_island_index @@ -435,11 +436,18 @@ def debug_draw(self): num_segments=12, ) if self.selected_object is not None: - aabb = self.selected_object.collision_shape_aabb + aabb = None + if isinstance( + self.selected_object, habitat_sim.physics.ManagedBulletRigidObject + ): + aabb = self.selected_object.collision_shape_aabb + else: + aabb = sutils.get_ao_root_bb(self.selected_object) dblr = self.sim.get_debug_line_render() dblr.push_transform(self.selected_object.transformation) dblr.draw_box(aabb.min, aabb.max, mn.Color4.magenta()) dblr.pop_transform() + ot = self.selected_object.translation # draw global coordinate axis dblr.draw_transformed_line( @@ -863,9 +871,16 @@ def key_press_event(self, event: Application.KeyEvent) -> None: obj_name = self.selected_object.handle.split("/")[-1].split("_:")[0] self.removed_clutter.append(obj_name) print(f"Removed {self.selected_object.handle}") - self.sim.get_rigid_object_manager().remove_object_by_handle( - self.selected_object.handle - ) + if isinstance( + self.selected_object, habitat_sim.physics.ManagedBulletRigidObject + ): + self.sim.get_rigid_object_manager().remove_object_by_handle( + self.selected_object.handle + ) + else: + self.sim.get_articulated_object_manager().remove_object_by_handle( + self.selected_object.handle + ) self.selected_object = None self.navmesh_config_and_recompute() @@ -968,13 +983,13 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: if mouse_cast_results.has_hits(): self.last_hit_details = mouse_cast_results.hits[0] hit_id = mouse_cast_results.hits[0].object_id - rom = self.sim.get_rigid_object_manager() - if hit_id == -1: + self.selected_object = sutils.get_obj_from_id(self.sim, hit_id) + if self.selected_object is None: print("This is the stage.") - elif rom.get_library_has_id(hit_id): - ro = rom.get_object_by_id(hit_id) - self.selected_object = ro - print(f"Rigid Object: {ro.handle}") + else: + print( + f"Object: {self.selected_object.handle} is {type(self.selected_object)}" + ) self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() From d94da56303689b9eb4e9700740ec74e40b258393 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 27 Feb 2024 16:54:32 -0800 Subject: [PATCH 079/153] rebase apps on habitat-lab main --- examples/spot_viewer.py | 7 +- examples/viewer.py | 185 +++++++++++++--------------------------- 2 files changed, 65 insertions(+), 127 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 653eac0832..b3fac577b7 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -301,7 +301,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: ] = {} self.removed_clutter = [] self.translation_speed = 0.05 - self.rotation_speed = 0.1 + self.rotation_speed = math.pi / 180.0 self.navmesh_dirty = False self.removed_objects_debug_frames = [] @@ -760,6 +760,7 @@ def move_selected_object( """ modify_buffer = translation is not None or rotation is not None if self.selected_object is not None and modify_buffer: + orig_mt = self.selected_object.motion_type self.selected_object.motion_type = habitat_sim.physics.MotionType.KINEMATIC if translation is not None: self.selected_object.translation = ( @@ -767,7 +768,7 @@ def move_selected_object( ) if rotation is not None: self.selected_object.rotation = rotation * self.selected_object.rotation - self.selected_object.motion_type = habitat_sim.physics.MotionType.STATIC + self.selected_object.motion_type = orig_mt self.navmesh_dirty = True self.modified_objects_buffer[ self.selected_object @@ -793,7 +794,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: else self.translation_speed * 2.0 ) obj_rotation_speed = ( - self.rotation_speed if not shift_pressed else self.rotation_speed * 2.0 + self.rotation_speed if not shift_pressed else self.rotation_speed * 4.0 ) if key == pressed.ESC: diff --git a/examples/viewer.py b/examples/viewer.py index 52714196e2..e926a0ac65 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -10,17 +10,17 @@ import sys import time from enum import Enum -from typing import Any, Callable, Dict, List, Optional, Tuple, Union +from typing import Any, Callable, Dict, List, Optional, Tuple flags = sys.getdlopenflags() sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle +import habitat.sims.habitat_simulator.sim_utilities as sutils import magnum as mn import numpy as np from habitat.datasets.rearrange.navmesh_utils import get_largest_island_index from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler -from habitat.sims.habitat_simulator.sim_utilities import get_bb_corners from magnum import shaders, text from magnum.platform.glfw import Application @@ -259,6 +259,7 @@ def __init__( # last clicked or None for stage self.selected_object = None self.selected_rec = None + self.ao_link_map = None # toggle a single simulation step at the next opportunity if not # simulating continuously. @@ -273,7 +274,6 @@ def __init__( self.reconfigure_sim() self.debug_semantic_colors = {} self.load_scene_filter_file() - self.ao_link_map = self.get_ao_link_id_map() # ----------------------------------------- # Clutter Generation Integration: @@ -510,65 +510,6 @@ def load_filtered_recs(self, filepath: Optional[str] = None) -> None: assert "height_filtered" in self.rec_filter_data print(f"Loaded filter annotations from {filepath}") - def get_ao_link_id_map(self) -> Dict[int, int]: - """ - Construct a map of ao_link object ids to their parent ao's object id. - NOTE: also maps ao's root object id to itself for ease of use. - - :param sim: The Simulator instance. - - :return: dictionary mapping ArticulatedLink object ids to their parent's object id. - """ - - aom = self.sim.get_articulated_object_manager() - ao_link_map: Dict[int, int] = {} - for ao in aom.get_objects_by_handle_substring().values(): - # add the ao itself for ease of use - ao_link_map[ao.object_id] = ao.object_id - # add the links - for link_id in ao.link_object_ids: - ao_link_map[link_id] = ao.object_id - - # print(f"ao_link_map = {ao_link_map}") - - return ao_link_map - - def get_object_by_handle( - self, handle: str - ) -> Union[ - habitat_sim.physics.ManagedRigidObject, - habitat_sim.physics.ManagedArticulatedObject, - ]: - """ - Get either a rigid or articulated object from the handle. If none is found, returns None. - """ - rom = self.sim.get_rigid_object_manager() - if rom.get_library_has_handle(handle): - return rom.get_object_by_handle(handle) - aom = self.sim.get_articulated_object_manager() - if aom.get_library_has_handle(handle): - return aom.get_object_by_handle(handle) - return None - - def get_object_by_id( - self, object_id: int - ) -> Union[ - habitat_sim.physics.ManagedRigidObject, - habitat_sim.physics.ManagedArticulatedObject, - ]: - """ - Get either a rigid or articulated object from the handle. If none is found, returns None. - """ - - rom = self.sim.get_rigid_object_manager() - if rom.get_library_has_id(object_id): - return rom.get_object_by_id(object_id) - aom = self.sim.get_articulated_object_manager() - if object_id in self.ao_link_map: - return aom.get_object_by_id(self.ao_link_map[object_id]) - - return None - def load_receptacles(self): """ Load all receptacle data and setup helper datastructures. @@ -581,8 +522,8 @@ def load_receptacles(self): ] for receptacle in self.receptacles: if receptacle not in self.rec_to_poh: - po_handle = self.get_object_by_handle( - receptacle.parent_object_handle + po_handle = sutils.get_obj_from_handle( + self.sim, receptacle.parent_object_handle ).creation_attributes.handle self.rec_to_poh[receptacle] = po_handle @@ -771,7 +712,9 @@ def debug_draw(self): rec_obj = self.get_object_by_handle(receptacle.parent_object_handle) key_points = [r_trans.translation] - key_points.extend(get_bb_corners(rec_obj.root_scene_node.cumulative_bb)) + key_points.extend( + sutils.get_bb_corners(rec_obj.root_scene_node.cumulative_bb) + ) in_view = False for ix, key_point in enumerate(key_points): @@ -1044,6 +987,8 @@ def reconfigure_sim( for composite_file in sim_settings["composite_files"]: self.replay_renderer.preload_file(composite_file) + self.ao_link_map = sutils.get_ao_link_id_map(self.sim) + Timer.start() self.step = -1 @@ -1354,7 +1299,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: raycast_results = self.sim.cast_ray(ray=ray) if raycast_results.has_hits(): - hit_object, ao_link = -1, -1 + ao_link = -1 hit_info = raycast_results.hits[0] if hit_info.object_id > habitat_sim.stage_id: @@ -1364,70 +1309,62 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: ao = ao_mngr.get_object_by_id(hit_info.object_id) ro = ro_mngr.get_object_by_id(hit_info.object_id) - if ro: - # if grabbed an object - hit_object = hit_info.object_id - object_pivot = ro.transformation.inverted().transform_point( - hit_info.point + if obj is None: + raise AssertionError( + "hit object_id is not valid. Did not find object or link." ) - object_frame = ro.rotation.inverted() - elif ao: - # if grabbed the base link - hit_object = hit_info.object_id - object_pivot = ao.transformation.inverted().transform_point( + + if obj.object_id == hit_info.object_id: + # ro or ao base + object_pivot = obj.transformation.inverted().transform_point( hit_info.point ) - object_frame = ao.rotation.inverted() - else: - for ao_handle in ao_mngr.get_objects_by_handle_substring(): - ao = ao_mngr.get_object_by_handle(ao_handle) - link_to_obj_ids = ao.link_object_ids - - if hit_info.object_id in link_to_obj_ids: - # if we got a link - ao_link = link_to_obj_ids[hit_info.object_id] - object_pivot = ( - ao.get_link_scene_node(ao_link) - .transformation.inverted() - .transform_point(hit_info.point) - ) - object_frame = ao.get_link_scene_node( - ao_link - ).rotation.inverted() - hit_object = ao.object_id - break - # done checking for AO - - if hit_object >= 0: - node = self.default_agent.scene_node - constraint_settings = physics.RigidConstraintSettings() - - constraint_settings.object_id_a = hit_object - constraint_settings.link_id_a = ao_link - constraint_settings.pivot_a = object_pivot - constraint_settings.frame_a = ( - object_frame.to_matrix() @ node.rotation.to_matrix() + object_frame = obj.rotation.inverted() + elif isinstance(obj, physics.ManagedArticulatedObject): + # link + ao_link = obj.link_object_ids[hit_info.object_id] + object_pivot = ( + obj.get_link_scene_node(ao_link) + .transformation.inverted() + .transform_point(hit_info.point) ) - constraint_settings.frame_b = node.rotation.to_matrix() - constraint_settings.pivot_b = hit_info.point + object_frame = obj.get_link_scene_node( + ao_link + ).rotation.inverted() + + print(f"Grabbed object {obj.handle}") + if ao_link >= 0: + print(f" link id {ao_link}") + + # setup the grabbing constraints + node = self.default_agent.scene_node + constraint_settings = physics.RigidConstraintSettings() + + constraint_settings.object_id_a = obj.object_id + constraint_settings.link_id_a = ao_link + constraint_settings.pivot_a = object_pivot + constraint_settings.frame_a = ( + object_frame.to_matrix() @ node.rotation.to_matrix() + ) + constraint_settings.frame_b = node.rotation.to_matrix() + constraint_settings.pivot_b = hit_info.point - # by default use a point 2 point constraint - if event.button == button.RIGHT: - constraint_settings.constraint_type = ( - physics.RigidConstraintType.Fixed - ) + # by default use a point 2 point constraint + if event.button == button.RIGHT: + constraint_settings.constraint_type = ( + physics.RigidConstraintType.Fixed + ) - grip_depth = ( - hit_info.point - render_camera.node.absolute_translation - ).length() + grip_depth = ( + hit_info.point - render_camera.node.absolute_translation + ).length() + + self.mouse_grabber = MouseGrabber( + constraint_settings, + grip_depth, + self.sim, + ) - self.mouse_grabber = MouseGrabber( - constraint_settings, - grip_depth, - self.sim, - ) - else: - logger.warn("Oops, couldn't find the hit object. That's odd.") # end if didn't hit the scene # end has raycast hit # end has physics enabled @@ -1445,7 +1382,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: if hit_id == -1: print("This is the stage.") else: - obj = self.get_object_by_id(hit_id) + obj = sutils.get_obj_from_id(self.sim, hit_id) self.selected_object = obj print(f"Object: {obj.handle}") if self.receptacles is not None: From f7f21ea7e69809773f4386e304beaa56ee7a39ea Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 27 Feb 2024 16:57:43 -0800 Subject: [PATCH 080/153] missed in merge --- examples/viewer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index e926a0ac65..eb2db2581a 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1379,7 +1379,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: self.selected_rec = None hit_id = self.mouse_cast_results.hits[0].object_id # right click in look mode to print object information - if hit_id == -1: + if hit_id == habitat_sim.stage_id: print("This is the stage.") else: obj = sutils.get_obj_from_id(self.sim, hit_id) From a6edc005397146150fd2ac66d91f3269122a687c Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Tue, 5 Mar 2024 11:50:58 -0500 Subject: [PATCH 081/153] --missed stuff in rebase --- examples/viewer.py | 380 +++++++++++++++++++++++++++++---------------- 1 file changed, 243 insertions(+), 137 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index eb2db2581a..4dd222604f 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -271,7 +271,7 @@ def __init__( self.tiled_sims: list[habitat_sim.simulator.Simulator] = None self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None self.replay_renderer: Optional[ReplayRenderer] = None - self.reconfigure_sim() + self.reconfigure_sim(mm) self.debug_semantic_colors = {} self.load_scene_filter_file() @@ -614,7 +614,6 @@ def debug_draw(self): debug_line_render = self.sim.get_debug_line_render() if self.contact_debug_draw: self.draw_contact_debug(debug_line_render) - if self.semantic_region_debug_draw: if len(self.debug_semantic_colors) != len(self.sim.semantic_scene.regions): for region in self.sim.semantic_scene.regions: @@ -622,95 +621,73 @@ def debug_draw(self): mn.Vector3(np.random.random(3)) ) self.draw_region_debug(debug_line_render) - - # mouse raycast circle - white = mn.Color4(mn.Vector3(1.0), 1.0) - if self.mouse_cast_results is not None and self.mouse_cast_results.has_hits(): - m_ray = self.mouse_cast_results.ray - self.sim.get_debug_line_render().draw_circle( - translation=m_ray.origin - + m_ray.direction - * self.mouse_cast_results.hits[0].ray_distance - * m_ray.direction.length(), - radius=0.005, - color=white, - normal=self.mouse_cast_results.hits[0].normal, + if self.receptacles is not None and self.display_receptacles: + if self.rec_filter_data is None and self.cpo_initialized: + self.compute_rec_filter_state( + access_threshold=self.rec_access_filter_threshold + ) + c_pos = self.render_camera.node.absolute_translation + c_forward = ( + self.render_camera.node.absolute_transformation().transform_vector( + mn.Vector3(0, 0, -1) + ) ) + for receptacle in self.receptacles: + rec_unique_name = self.get_rec_instance_name(receptacle) + # filter all non-active receptacles + if ( + self.rec_filter_data is not None + and not self.show_filtered + and rec_unique_name not in self.rec_filter_data["active"] + ): + continue - if gt_raycast_results is not None and self.debug_draw_raycasts: - scene_bb = self.sim.get_active_scene_graph().get_root_node().cumulative_bb - inflated_scene_bb = scene_bb.scaled(mn.Vector3(1.25)) - inflated_scene_bb = mn.Range3D.from_center( - scene_bb.center(), inflated_scene_bb.size() / 2.0 - ) - self.sim.get_debug_line_render().draw_box( - inflated_scene_bb.min, inflated_scene_bb.max, white - ) - if self.sim.get_rigid_object_manager().get_num_objects() == 0: - self.collision_proxy_obj = ( - self.sim.get_rigid_object_manager().add_object_by_template_handle( - obj_temp_handle - ) - # only display receptacles within 4 meters - if mn.math.dot((c_to_r).normalized(), c_forward) > 0.7: - # handle coloring - rec_color = None - if self.selected_rec == receptacle: - # white - rec_color = mn.Color4.cyan() - elif ( - self.cpo_initialized - and self.rec_color_mode != RecColorMode.DEFAULT - ): - if self.rec_color_mode == RecColorMode.GT_STABILITY: - rec_color = rg_lerp.at( - rec_dat["shape_id_results"]["gt"][ - "stability_results" - ]["success_ratio"] - ) - elif self.rec_color_mode == RecColorMode.GT_ACCESS: - rec_color = rg_lerp.at( - rec_dat["shape_id_results"]["gt"]["access_results"][ - "receptacle_access_score" - ] - ) - elif self.rec_color_mode == RecColorMode.PR_STABILITY: - rec_color = rg_lerp.at( - rec_dat["shape_id_results"]["pr0"][ - "stability_results" - ]["success_ratio"] - ) - elif self.rec_color_mode == RecColorMode.PR_ACCESS: - rec_color = rg_lerp.at( - rec_dat["shape_id_results"]["pr0"][ - "access_results" - ]["receptacle_access_score"] - ) - elif self.rec_color_mode == RecColorMode.FILTERING: - if rec_unique_name in self.rec_filter_data["active"]: - rec_color = mn.Color4.green() - elif ( - rec_unique_name - in self.rec_filter_data["manually_filtered"] - ): - rec_color = mn.Color4.yellow() - elif ( - rec_unique_name - in self.rec_filter_data["access_filtered"] - ): - rec_color = mn.Color4.red() - elif ( - rec_unique_name - in self.rec_filter_data["stability_filtered"] - ): - rec_color = mn.Color4.magenta() - elif ( - rec_unique_name - in self.rec_filter_data["height_filtered"] - ): - rec_color = mn.Color4.blue() + rec_dat = None + if self.cpo_initialized: + rec_dat = self._cpo.gt_data[self.rec_to_poh[receptacle]][ + "receptacles" + ][receptacle.name] + + r_trans = receptacle.get_global_transform(self.sim) + # display point samples for selected object + if ( + rec_dat is not None + and self.display_selected_stability_samples + and self.selected_object is not None + and self.selected_object.handle == receptacle.parent_object_handle + ): + # display colored circles for stability samples on the selected object + point_metric_dat = rec_dat["shape_id_results"]["gt"][ + "access_results" + ]["receptacle_point_access_scores"] + if self.rec_color_mode == RecColorMode.GT_STABILITY: + point_metric_dat = rec_dat["shape_id_results"]["gt"][ + "stability_results" + ]["point_stabilities"] + elif self.rec_color_mode == RecColorMode.PR_STABILITY: + point_metric_dat = rec_dat["shape_id_results"]["pr0"][ + "stability_results" + ]["point_stabilities"] + elif self.rec_color_mode == RecColorMode.PR_ACCESS: + point_metric_dat = rec_dat["shape_id_results"]["pr0"][ + "access_results" + ]["receptacle_point_access_scores"] + + for point_metric, point in zip( + point_metric_dat, + rec_dat["sample_points"], + ): + self.sim.get_debug_line_render().draw_circle( + translation=r_trans.transform_point(point), + radius=0.02, + normal=mn.Vector3(0, 1, 0), + color=rg_lerp.at(point_metric), + num_segments=12, + ) - rec_obj = self.get_object_by_handle(receptacle.parent_object_handle) + rec_obj = sutils.get_obj_from_handle( + self.sim, receptacle.parent_object_handle + ) key_points = [r_trans.translation] key_points.extend( sutils.get_bb_corners(rec_obj.root_scene_node.cumulative_bb) @@ -1143,50 +1120,186 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # urdf_file_path = self.cached_urdf # else: # urdf_file_path = input("Load URDF: provide a URDF filepath:").strip() - - if not urdf_file_path: - logger.warn("Load URDF: no input provided. Aborting.") - elif not urdf_file_path.endswith((".URDF", ".urdf")): - logger.warn("Load URDF: input is not a URDF. Aborting.") - elif os.path.exists(urdf_file_path): - self.cached_urdf = urdf_file_path - aom = self.sim.get_articulated_object_manager() - ao = aom.add_articulated_object_from_urdf( - urdf_file_path, - fixed_base, - 1.0, - 1.0, - True, - maintain_link_order=False, - intertia_from_urdf=False, - ) - ao.translation = ( - self.default_agent.scene_node.transformation.transform_point( - [0.0, 1.0, -1.5] - ) - ) - # check removal and auto-creation - joint_motor_settings = habitat_sim.physics.JointMotorSettings( - position_target=0.0, - position_gain=1.0, - velocity_target=0.0, - velocity_gain=1.0, - max_impulse=1000.0, - ) - existing_motor_ids = ao.existing_joint_motor_ids - for motor_id in existing_motor_ids: - ao.remove_joint_motor(motor_id) - ao.create_all_motors(joint_motor_settings) - else: - logger.warn("Load URDF: input file not found. Aborting.") + # if not urdf_file_path: + # logger.warn("Load URDF: no input provided. Aborting.") + # elif not urdf_file_path.endswith((".URDF", ".urdf")): + # logger.warn("Load URDF: input is not a URDF. Aborting.") + # elif os.path.exists(urdf_file_path): + # self.cached_urdf = urdf_file_path + # aom = self.sim.get_articulated_object_manager() + # ao = aom.add_articulated_object_from_urdf( + # urdf_file_path, + # fixed_base, + # 1.0, + # 1.0, + # True, + # maintain_link_order=False, + # intertia_from_urdf=False, + # ) + # ao.translation = ( + # self.default_agent.scene_node.transformation.transform_point( + # [0.0, 1.0, -1.5] + # ) + # ) + # # check removal and auto-creation + # joint_motor_settings = habitat_sim.physics.JointMotorSettings( + # position_target=0.0, + # position_gain=1.0, + # velocity_target=0.0, + # velocity_gain=1.0, + # max_impulse=1000.0, + # ) + # existing_motor_ids = ao.existing_joint_motor_ids + # for motor_id in existing_motor_ids: + # ao.remove_joint_motor(motor_id) + # ao.create_all_motors(joint_motor_settings) + # else: + # logger.warn("Load URDF: input file not found. Aborting.") elif key == pressed.M: self.cycle_mouse_mode() logger.info(f"Command: mouse mode set to {self.mouse_interaction}") elif key == pressed.V: - self.invert_gravity() - logger.info("Command: gravity inverted") + # load receptacles and toggle visibilty or color mode (+SHIFT) + if self.receptacles is None: + self.load_receptacles() + + if shift_pressed: + self.rec_color_mode = RecColorMode( + (self.rec_color_mode.value + 1) % len(RecColorMode) + ) + print(f"self.rec_color_mode = {self.rec_color_mode}") + self.display_receptacles = True + else: + self.display_receptacles = not self.display_receptacles + print(f"self.display_receptacles = {self.display_receptacles}") + + elif key == pressed.F: + # toggle, load(+ALT), or save(+SHIFT) filtering + if shift_pressed and self.rec_filter_data is not None: + self.export_filtered_recs(self.rec_filter_path) + elif alt_pressed: + self.load_filtered_recs(self.rec_filter_path) + else: + self.show_filtered = not self.show_filtered + print(f"self.show_filtered = {self.show_filtered}") + + elif key == pressed.U: + rom = self.sim.get_rigid_object_manager() + # add objects to the selected receptacle or remove al objects + if shift_pressed: + # remove all + print(f"Removing {len(self.clutter_object_instances)} clutter objects.") + for obj in self.clutter_object_instances: + rom.remove_object_by_handle(obj.handle) + self.clutter_object_initial_states.clear() + self.clutter_object_instances.clear() + else: + # try to sample an object from the selected object receptacles + rec_set = None + if alt_pressed: + # use all active filter recs + rec_set = [ + rec + for rec in self.receptacles + if rec.unique_name in self.rec_filter_data["active"] + ] + elif self.selected_rec is not None: + rec_set = [self.selected_rec] + elif self.selected_object is not None: + rec_set = [ + rec + for rec in self.receptacles + if self.selected_object.handle == rec.parent_object_handle + ] + if rec_set is not None: + if len(self.clutter_object_handles) == 0: + for obj_name in self.clutter_object_set: + matching_handles = self.sim.metadata_mediator.object_template_manager.get_template_handles( + obj_name + ) + assert ( + len(matching_handles) > 0 + ), f"No matching template for '{obj_name}' in the dataset." + self.clutter_object_handles.append(matching_handles[0]) + + rec_set_unique_names = [rec.unique_name for rec in rec_set] + obj_samp = ObjectSampler( + self.clutter_object_handles, + ["rec set"], + orientation_sample="up", + num_objects=(1, 10), + ) + rec_set_obj = hab_receptacle.ReceptacleSet( + "rec set", [""], [], rec_set_unique_names, [] + ) + obj_count_dict = {rec.unique_name: 200 for rec in rec_set} + recep_tracker = hab_receptacle.ReceptacleTracker( + obj_count_dict, + {"rec set": rec_set_obj}, + ) + new_objs = obj_samp.sample( + self.sim, recep_tracker, [], snap_down=True + ) + for obj, rec in new_objs: + self.clutter_object_instances.append(obj) + self.clutter_object_initial_states.append( + (obj.translation, obj.rotation) + ) + print(f"Sampled '{obj.handle}' in '{rec.unique_name}'") + else: + print("No object selected, cannot sample clutter.") + + elif key == pressed.O: + if shift_pressed: + # move non-proxy objects in/out of visible space + self.original_objs_visible = not self.original_objs_visible + print(f"self.original_objs_visible = {self.original_objs_visible}") + if not self.original_objs_visible: + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + obj.translation = obj.translation + mn.Vector3(200, 0, 0) + obj.motion_type = habitat_sim.physics.MotionType.STATIC + else: + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + obj.translation = obj.translation - mn.Vector3(200, 0, 0) + obj.motion_type = habitat_sim.physics.MotionType.STATIC + + else: + if self.col_proxy_objs is None: + self.col_proxy_objs = [] + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + # add a new proxy object + self.col_proxy_objs.append(self.add_col_proxy_object(obj)) + else: + self.col_proxies_visible = not self.col_proxies_visible + print(f"self.col_proxies_visible = {self.col_proxies_visible}") + + # make the proxies visible or not by moving them + if not self.col_proxies_visible: + for obj in self.col_proxy_objs: + obj.translation = obj.translation + mn.Vector3(200, 0, 0) + else: + for obj in self.col_proxy_objs: + obj.translation = obj.translation - mn.Vector3(200, 0, 0) + elif key == pressed.N: # (default) - toggle navmesh visualization # NOTE: (+ALT) - re-sample the agent position on the NavMesh @@ -1303,11 +1416,9 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: hit_info = raycast_results.hits[0] if hit_info.object_id > habitat_sim.stage_id: - # we hit an non-staged collision object - ro_mngr = self.sim.get_rigid_object_manager() - ao_mngr = self.sim.get_articulated_object_manager() - ao = ao_mngr.get_object_by_id(hit_info.object_id) - ro = ro_mngr.get_object_by_id(hit_info.object_id) + obj = sutils.get_obj_from_id( + self.sim, hit_info.object_id, self.ao_link_map + ) if obj is None: raise AssertionError( @@ -1864,11 +1975,6 @@ def run_cpo_for_obj(obj_handle): action="store_true", help="disable physics simulation (default: False)", ) - parser.add_argument( - "--reorient-object", - action="store_true", - help="reorient the object based on the values in the config file.", - ) parser.add_argument( "--use-default-lighting", action="store_true", @@ -1929,9 +2035,9 @@ def run_cpo_for_obj(obj_handle): sim_settings["composite_files"] = args.composite_files sim_settings["window_width"] = args.width sim_settings["window_height"] = args.height + sim_settings["rec_filter_file"] = args.rec_filter_file sim_settings["default_agent_navmesh"] = False sim_settings["enable_hbao"] = args.hbao - sim_settings["clear_color"] = mn.Color4.magenta() # don't need auto-navmesh sim_settings["default_agent_navmesh"] = False From 35f3173571c75d903b5075e935b5ee101afe6d6a Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Tue, 5 Mar 2024 11:52:28 -0500 Subject: [PATCH 082/153] --rebase issue --- tools/collision_shape_automation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py index df6cd3ecfc..d279649fbe 100644 --- a/tools/collision_shape_automation.py +++ b/tools/collision_shape_automation.py @@ -15,12 +15,12 @@ coacd_imported = False try: import coacd + import trimesh coacd_imported = True except Exception: coacd_imported = False print("Failed to import coacd, is it installed? Linux only: 'pip install coacd'") -import trimesh # not adding this causes some failures in mesh import flags = sys.getdlopenflags() From 9935ec5c9f924d0decfdf0ce0936671093e94e97 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Tue, 12 Mar 2024 13:27:18 -0400 Subject: [PATCH 083/153] --minor cleanup; add scene ID to screen text --- examples/viewer.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 4dd222604f..7f1bd78e10 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -995,10 +995,9 @@ def move_and_look(self, repetitions: int) -> None: if repetitions == 0: return - key = Application.KeyEvent.Key agent = self.sim.agents[self.agent_id] - press: Dict[key.key, bool] = self.pressed - act: Dict[key.key, str] = self.key_to_action + press: Dict[Application.KeyEvent.Key.key, bool] = self.pressed + act: Dict[Application.KeyEvent.Key.key, str] = self.key_to_action action_queue: List[str] = [act[k] for k, v in press.items() if v] @@ -1701,6 +1700,7 @@ def draw_text(self, sensor_spec): self.window_text.render( f""" {self.fps} FPS +Scene ID : {self.cfg.sim_cfg.scene_id} Sensor Type: {sensor_type_string} Sensor Subtype: {sensor_subtype_string} Mouse Interaction Mode: {mouse_mode_string} @@ -2036,7 +2036,6 @@ def run_cpo_for_obj(obj_handle): sim_settings["window_width"] = args.width sim_settings["window_height"] = args.height sim_settings["rec_filter_file"] = args.rec_filter_file - sim_settings["default_agent_navmesh"] = False sim_settings["enable_hbao"] = args.hbao # don't need auto-navmesh From 7bb640063855d810b46967defdfddf10968103ba Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Tue, 12 Mar 2024 13:42:56 -0400 Subject: [PATCH 084/153] --fix for embedded paths --- examples/viewer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index 7f1bd78e10..9ef990701a 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1700,7 +1700,7 @@ def draw_text(self, sensor_spec): self.window_text.render( f""" {self.fps} FPS -Scene ID : {self.cfg.sim_cfg.scene_id} +Scene ID : {os.path.split(self.cfg.sim_cfg.scene_id)[1].split('.scene_instance')[0]} Sensor Type: {sensor_type_string} Sensor Subtype: {sensor_subtype_string} Mouse Interaction Mode: {mouse_mode_string} From 983a4c2f64d140e5a93d2bdacaddbf99a9d10b06 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Wed, 13 Mar 2024 13:06:14 -0400 Subject: [PATCH 085/153] --add command line to not build navmesh For free-look-like behavior --- examples/viewer.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/examples/viewer.py b/examples/viewer.py index 9ef990701a..ba7a90d311 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -303,6 +303,7 @@ def __init__( if ( not self.sim.pathfinder.is_loaded and self.cfg.sim_cfg.scene_id.lower() != "none" + and not self.sim_settings["viewer_ignore_navmesh"] ): self.navmesh_config_and_recompute() @@ -2002,6 +2003,12 @@ def run_cpo_for_obj(obj_handle): nargs="*", help="Composite files that the batch renderer will use in-place of simulation assets to improve memory usage and performance. If none is specified, the original scene files will be loaded from disk.", ) + parser.add_argument( + "--no-navmesh", + default=False, + action="store_true", + help="Don't build navmesh.", + ) parser.add_argument( "--width", default=1080, @@ -2037,6 +2044,7 @@ def run_cpo_for_obj(obj_handle): sim_settings["window_height"] = args.height sim_settings["rec_filter_file"] = args.rec_filter_file sim_settings["enable_hbao"] = args.hbao + sim_settings["viewer_ignore_navmesh"] = args.no_navmesh # don't need auto-navmesh sim_settings["default_agent_navmesh"] = False From 0f52d1c62ed208e7677ad5188070b5994000f861 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 14 Aug 2023 17:03:44 -0700 Subject: [PATCH 086/153] add blender armature to urdf converter tool --- tools/blender_armature_to_urdf.py | 670 ++++++++++++++++++++++++++++++ 1 file changed, 670 insertions(+) create mode 100644 tools/blender_armature_to_urdf.py diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py new file mode 100644 index 0000000000..8565591eee --- /dev/null +++ b/tools/blender_armature_to_urdf.py @@ -0,0 +1,670 @@ +# 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 os +import xml.dom.minidom as minidom +import xml.etree.ElementTree as ET +from collections import defaultdict +from typing import Any, Dict, List, Tuple + +import bpy +from mathutils import Quaternion, Vector + +# Colors from https://colorbrewer2.org/ +colors = [ + (166, 206, 227), + (31, 120, 180), + (178, 223, 138), + (51, 160, 44), + (251, 154, 153), + (227, 26, 28), + (253, 191, 111), + (255, 127, 0), + (202, 178, 214), + (106, 61, 154), + (255, 255, 153), + (177, 89, 40), +] +colors = [(c[0] / 255, c[1] / 255, c[2] / 255) for c in colors] +next_color = 0 + +# state vars +armature = None +counter = 0 +links = [] +joints = [] +bones_to_meshes: Dict[Any, List[Any]] = defaultdict( + list +) # maps bones to lists of mesh objects + +# constants +LINK_NAME_FORMAT = "{bone_name}" +JOINT_NAME_FORMAT = "{bone_name}" +ORIGIN_NODE_FLOAT_PRECISION = 6 +ORIGIN_NODE_FORMAT = "{{:.{0}f}} {{:.{0}f}} {{:.{0}f}}".format( + ORIGIN_NODE_FLOAT_PRECISION +) +ZERO_ORIGIN_NODE = lambda: ET.fromstring('') +INERTIA_NODE_FMT = '' +AXIS_NODE_FORMAT = lambda: ET.fromstring('') +BASE_LIMIT_NODE_STR = None + + +def set_base_limit_str(effort, velocity): + """ + Default effort and velocity limits for Joints. + """ + global BASE_LIMIT_NODE_STR + BASE_LIMIT_NODE_STR = ''.format( + effort, velocity + ) + + +def deselect_all() -> None: + """ + Deselect all objects. + """ + for obj in bpy.context.selected_objects: + obj.select_set(False) + + +def is_mesh(obj) -> bool: + """ + Is the object a MESH? + """ + return obj.type == "MESH" + + +def is_collision_mesh(mesh_obj) -> bool: + """ + Is the object a collision mesh. + """ + return "collision" in mesh_obj.name + + +def get_mesh_heirarchy( + mesh_obj, + select_set: bool = True, + include_non_collision: bool = True, + include_collison: bool = False, +) -> List[Any]: + """ + Select all MESH objects in the heirarchy specifically targeting or omitting meshes with "collision" in the name. + + :param mesh_obj: The Blender mesh object. + :param select_set: Whether or not to select the objects as well as recording them. + :param include_non_collision: Include objects without 'collision' in the name. + :param include_collison: Include objects with 'collision' in the name. + + :return: The list of Blender mesh objects. + """ + selected_objects = [] + is_col_mesh = is_collision_mesh(mesh_obj) + if is_mesh(mesh_obj) and ( + (is_col_mesh and include_collison) + or (include_non_collision and not is_col_mesh) + ): + selected_objects.append(mesh_obj) + if select_set: + mesh_obj.select_set(True) + for child in mesh_obj.children: + if child.type != "ARMATURE": + selected_objects.extend( + get_mesh_heirarchy( + child, select_set, include_non_collision, include_collison + ) + ) + return selected_objects + + +def walk_armature(this_bone, handler): + """ + Recursively apply a handler function to bone children to traverse the armature. + """ + handler(this_bone) + for child in this_bone.children: + walk_armature(child, handler) + + +def bone_info(bone): + """ + Print relevant bone info to console. + """ + print(" bone info") + print(f" name: {bone.name}") + print(" children") + for child in bone.children: + print(f" - {child.name}") + + +def node_info(node): + """ + Print relevant info about an object node. + """ + print(" node info") + print(f" name: {node.name}") + print(" children") + for child in node.children: + print(f" - {child.name}") + + +def get_origin_from_matrix(M): + """ + Construct a URDF 'origin' element from a Matrix by separating translation from rotation and converting to Euler. + """ + translation = M.to_translation() + euler = M.to_euler() + origin_xml_node = ET.Element("origin") + origin_xml_node.set("rpy", ORIGIN_NODE_FORMAT.format(euler.x, euler.y, euler.z)) + origin_xml_node.set( + "xyz", ORIGIN_NODE_FORMAT.format(translation.x, translation.y, translation.z) + ) + + return origin_xml_node + + +def get_next_color() -> Tuple[int, int, int]: + """ + Global function to get the next color in the colors list. + """ + global next_color + this_color = colors[next_color % len(colors)] + next_color += 1 + return this_color + + +def add_color_material_to_visual(color, xml_visual) -> None: + """ + Add a color material to a visual node. + """ + this_xml_material = ET.Element("material") + this_xml_material.set("name", "mat_col_{}".format(color)) + this_xml_color = ET.Element("color") + this_xml_color.set("rgba", "{:.2f} {:.2f} {:.2f} 1.0".format(*color)) + this_xml_material.append(this_xml_color) + xml_visual.append(this_xml_material) + + +def bone_to_urdf(this_bone, collision_visuals=True, joint_visual=True): + """This function extracts the basic properties of the bone and populates + links and joint lists with the corresponding urdf nodes""" + + global counter + + # Create the joint xml node + if this_bone.parent: + this_xml_link = create_bone_link(this_bone) + else: + this_xml_link = create_root_bone_link(this_bone) + + # NOTE: default intertia (assume overriden automatically in-engine) + this_xml_link.append(ET.fromstring(INERTIA_NODE_FMT.format(1.0, 0, 0, 1.0, 0, 1.0))) + + # NOTE: default unit mass TODO: estimate somehow? + this_xml_link.append(ET.fromstring(''.format(1.0))) + + # TODO: scrape the list of mesh filenames which would be generated by an export. + collision_objects = [] + for mesh_obj in bones_to_meshes[this_bone.name]: + collision_objects.extend( + get_mesh_heirarchy( + mesh_obj, + select_set=False, + include_collison=True, + include_non_collision=False, + ) + ) + if mesh_obj.parent is not None and mesh_obj.parent.type == "ARMATURE": + # this object is the mesh name for export, so use it + # Create the visual node + this_xml_visual = ET.Element("visual") + this_xml_mesh_geom = ET.Element("geometry") + this_xml_mesh = ET.Element("mesh") + this_xml_mesh.set("filename", f"{mesh_obj.name}.glb") + this_xml_mesh.set("scale", "1.0 1.0 1.0") + this_xml_mesh_geom.append(this_xml_mesh) + # NOTE: we can use zero because we reset the origin for the meshes before exporting them to glb + this_xml_visual.append(ZERO_ORIGIN_NODE()) + this_xml_visual.append(this_xml_mesh_geom) + if not collision_visuals: + this_xml_link.append(this_xml_visual) + + # NOTE: visual debugging tool to add a box at the joint pivot locations + if joint_visual: + this_xml_visual = ET.Element("visual") + this_xml_test_geom = ET.Element("geometry") + this_xml_box = ET.Element("box") + this_xml_box.set("size", f"{0.1} {0.1} {0.1}") + this_xml_test_geom.append(this_xml_box) + this_xml_visual.append(ZERO_ORIGIN_NODE()) + this_xml_visual.append(this_xml_test_geom) + this_xml_link.append(this_xml_visual) + + # NOTE: color each link's collision shapes for debugging + this_color = get_next_color() + + for col in collision_objects: + assert ( + "collision_box" in col.name or "collision_cylinder" in col.name + ), "Only supporting collision boxes and cylinders currently." + + set_obj_origin_to_center(col) + clear_obj_transform( + col, apply=True, include_scale_apply=False, include_rot_apply=False + ) + set_obj_origin_to_xyz(col, col.parent.matrix_world.translation) + clear_obj_transform(col) + set_obj_origin_to_center(col) + + # Create the collision node + this_xml_collision = ET.Element("collision") + if collision_visuals: + this_xml_collision = ET.Element("visual") + add_color_material_to_visual(this_color, this_xml_collision) + this_xml_col_geom = ET.Element("geometry") + xml_shape = None + if "collision_box" in col.name: + this_xml_box = ET.Element("box") + box_size = col.scale + this_xml_box.set("size", f"{box_size.x} {box_size.y} {box_size.z}") + xml_shape = this_xml_box + if "collision_cylinder" in col.name: + this_xml_cyl = ET.Element("cylinder") + scale = col.scale + # radius XY axis scale must match + assert scale.x == scale.y, "XY dimensions must match. Used as radius." + this_xml_cyl.set("radius", f"{scale.x/2.0}") + # NOTE: assume Z axis is length of the cylinder + this_xml_cyl.set("length", f"{scale.z}") + xml_shape = this_xml_cyl + this_xml_col_geom.append(xml_shape) + # first get the rotation + xml_origin = get_origin_from_matrix(col.matrix_local) + # then get local translation + col_link_position = col.location + xml_origin.set( + "xyz", + ORIGIN_NODE_FORMAT.format( + col_link_position.x, col_link_position.y, col_link_position.z + ), + ) + this_xml_collision.append(xml_origin) + this_xml_collision.append(this_xml_col_geom) + this_xml_link.append(this_xml_collision) + + if not this_bone.children: + pass + # We reached the end of the chain. + + counter += 1 + + +def create_root_bone_link(this_bone): + """ + Construct the root link element from a bone. + Called for bones with no parent (i.e. the root node) + """ + xml_link = ET.Element("link") + xml_link_name = this_bone.name + xml_link.set("name", xml_link_name) + links.append(xml_link) + + this_bone.name = xml_link_name + return xml_link + + +def get_origin_from_bone(bone): + """ + Construct an origin element for a joint from a bone. + """ + translation = ( + bone.matrix_local.to_translation() - bone.parent.matrix_local.to_translation() + ) + + origin_xml_node = ET.Element("origin") + origin_xml_node.set("rpy", "0 0 0") + origin_xml_node.set( + "xyz", ORIGIN_NODE_FORMAT.format(translation.x, translation.y, translation.z) + ) + + return origin_xml_node + + +def create_bone_link(this_bone): + """ + Construct Link and Joint elements from a bone. + """ + global counter + + # construct limits and joint type from animation frames + bone_limits = get_anim_limits_info(this_bone) + + # Get bone properties + parent_bone = this_bone.parent + base_joint_name = JOINT_NAME_FORMAT.format( + counter=counter, bone_name=this_bone.name + ) + + # ------------- Create joint-------------- + + joint = ET.Element("joint") + joint.set("name", base_joint_name) + + # create origin node + origin_xml_node = get_origin_from_bone(this_bone) + + # create parent node + parent_xml_node = ET.Element("parent") + parent_xml_node.set("link", parent_bone.name) + + xml_link = ET.Element("link") + xml_link_name = this_bone.name + xml_link.set("name", xml_link_name) + links.append(xml_link) + + # create child node + child_xml_node = ET.Element("child") + child_xml_node.set("link", xml_link_name) + + joint.append(parent_xml_node) + joint.append(child_xml_node) + + # create limits node + limit_node = ET.fromstring(BASE_LIMIT_NODE_STR) + + axis = Vector() + + # Revolute + if len(bone_limits["lower_limit"]) == 4: + joint.set("type", "revolute") + begin = Quaternion(bone_limits["lower_limit"]) + end = Quaternion(bone_limits["upper_limit"]) + rest = Quaternion() + diff = begin.rotation_difference(end) + axis, angle = diff.to_axis_angle() + # NOTE: rest pose could be applied to the bone resulting in an additional rotation stored in the matrix property + rest_correction = this_bone.matrix + # NOTE: Blender bones and armature are always Y-up, so we need to rotate the axis into URDF coordinate space (Z-up) + bone_axis = this_bone.vector + to_z_up = bone_axis.rotation_difference(Vector([0, 0, 1])) + # apply all rotations to arrive at the URDF Joint axis + axis = rest_correction @ (to_z_up @ axis) + rest_diff = begin.rotation_difference(rest) + rest_axis, rest_angle = rest_diff.to_axis_angle() + limit_node.set("lower", f"{-rest_angle}") + limit_node.set("upper", f"{angle-rest_angle}") + + # Prismatic + if len(bone_limits["lower_limit"]) == 3: + joint.set("type", "prismatic") + upper_vec = Vector(bone_limits["upper_limit"]) + lower_vec = Vector(bone_limits["lower_limit"]) + displacement = upper_vec - lower_vec + axis = displacement.normalized() + limit_node.set("lower", f"{-lower_vec.length}") + limit_node.set("upper", f"{upper_vec.length}") + + xml_axis = AXIS_NODE_FORMAT() + xml_axis.set("xyz", ORIGIN_NODE_FORMAT.format(axis.x, axis.y, axis.z)) + + joint.append(xml_axis) + joint.append(limit_node) + joint.append(origin_xml_node) + joints.append(joint) + ret_link = xml_link + + return ret_link + + +# ========================================== + + +def set_obj_origin_to_center(obj) -> None: + """ + Set object origin to it's own center. + """ + deselect_all() + obj.select_set(True) + bpy.ops.object.origin_set(type="ORIGIN_GEOMETRY", center="MEDIAN") + + +def set_obj_origin_to_xyz(obj, xyz) -> None: + """ + Set object origin to a global xyz location. + """ + deselect_all() + bpy.context.scene.cursor.location = xyz + obj.select_set(True) + bpy.ops.object.origin_set(type="ORIGIN_CURSOR", center="MEDIAN") + + +def set_obj_origin_to_bone(obj, bone): + """ + Set the object origin to the bone transformation. + """ + set_obj_origin_to_xyz(obj, bone.matrix_local.translation) + + +def clear_obj_transform( + arm, apply=False, include_scale_apply=True, include_rot_apply=True +): + """ + Clear the armature transform to align it with the origin. + """ + deselect_all() + arm.select_set(True) + if apply: + bpy.ops.object.transform_apply( + location=True, rotation=include_rot_apply, scale=include_scale_apply + ) + else: + bpy.ops.object.location_clear(clear_delta=False) + + +def get_anim_limits_info(bone): + """ + Get limits info from animation action tracks. + """ + bone_limits = {"rest_pose": [], "lower_limit": [], "upper_limit": []} + if "root" in bone.name: + # no joint data defined for the root + return bone_limits + is_prismatic = False + is_revolute = False + + for ac_key in bpy.data.actions: + if bone.name in ac_key: + key_match = [key for key in bone_limits if key in ac_key][0] + limit_list = [] + for _fkey, fcurve in bpy.data.actions[ac_key].fcurves.items(): + assert ( + len(fcurve.keyframe_points) == 1 + ), "Expecting one keyframe per track." + index = fcurve.array_index + value = fcurve.keyframe_points[0].co[1] + if "quaternion" in fcurve.data_path: + if len(limit_list) == 0: + limit_list = [0, 0, 0, 0] + is_revolute = True + if "location" in fcurve.data_path: + if len(limit_list) == 0: + limit_list = [0, 0, 0] + is_prismatic = True + limit_list[index] = value + bone_limits[key_match] = limit_list + assert ( + is_prismatic or is_revolute + ), f"Bone {bone.name} does not have animation data." + assert not ( + is_prismatic and is_revolute + ), f"Bone {bone.name} has both rotation and translation defined." + return bone_limits + + +def get_parent_bone(obj): + """ + Climb the node tree looking for the parent bone of an object. + Return the parent bone or None if a parent bone does not exist. + """ + if obj.parent_bone != "": + return armature.data.bones[obj.parent_bone] + if obj.parent is None: + return None + return get_parent_bone(obj.parent) + + +def get_root_bone(): + """ + Find the root bone. + """ + root_bone = None + for b in armature.data.bones: + if not b.parent: + assert root_bone is None, "More than one root bone found." + root_bone = b + return root_bone + + +def get_armature(): + """ + Search the objects for an armature object. + """ + for obj in bpy.data.objects: + if obj.type == "ARMATURE": + return obj + return None + + +def export(dirpath, settings, export_meshes: bool = True): + """ + Run the Armature to URDF converter and export the .urdf file. + Recursively travserses the armature bone tree and constructs Links and Joints. + Note: This process is destructive and requires undo or revert in the editor after use. + """ + + global LINK_NAME_FORMAT, JOINT_NAME_FORMAT, armature, root_bone, links, joints, counter + counter = 0 + links = [] + joints = [] + + # get the armature + armature = settings.get("armature", bpy.data.objects["Armature"]) + + # find the root bone + root_bone = get_root_bone() + + if "link_name_format" in settings: + LINK_NAME_FORMAT = settings["link_name_format"] + + if "joint_name_format" in settings: + JOINT_NAME_FORMAT = settings["joint_name_format"] + + # set the defaults to 100 T units and 3 units/sec (meters or radians) + effort, velocity = (100, 3) + if "def_limit_effort" in settings: + effort = settings["def_limit_effort"] + if "def_limit_vel" in settings: + velocity = settings["def_limit_vel"] + set_base_limit_str(effort, velocity) + + # clear the armature transform to remove unwanted transformations for later + clear_obj_transform(armature, apply=True) + + # print all mesh object parents, reset origins for mesh export and transformation registery, collect bone to mesh map + root_node = None + for obj in bpy.data.objects: + if obj.type == "MESH": + parent_bone = get_parent_bone(obj) + set_obj_origin_to_bone(obj, parent_bone) + print(f"MESH: {obj.name}") + if obj.parent is not None: + print(f" -p> {obj.parent.name}") + if obj.parent_bone != "": + bones_to_meshes[obj.parent_bone].append(obj) + print(f" -pb> {obj.parent_bone}") + elif obj.type == "EMPTY": + print(f"EMPTY: {obj.name}") + if obj.parent is None and len(obj.children) > 0: + print(" --IS ROOT") + root_node = obj + + # make export directory for the object + assert root_node is not None, "No root node, aborting." + final_out_path = os.path.join(dirpath, f"{root_node.name}") + os.makedirs(final_out_path, exist_ok=True) + + # export mesh components + if export_meshes: + for mesh_list in bones_to_meshes.values(): + for mesh_obj in mesh_list: + if mesh_obj.parent is not None and mesh_obj.parent.type == "ARMATURE": + clear_obj_transform(mesh_obj) + deselect_all() + get_mesh_heirarchy(mesh_obj) + bpy.ops.export_scene.gltf( + filepath=os.path.join(final_out_path, mesh_obj.name), + use_selection=True, + export_yup=False, + ) + return None + + # print("------------------------") + # print("Bone info recursion:") + # walk_armature(root_bone, bone_info) + # print("------------------------") + # print("Node info recursion:") + # walk_armature(root_node, node_info) + # print("------------------------") + + # Recursively generate the xml elements + walk_armature(root_bone, bone_to_urdf) + + # add all the joints and links to the root + root_xml = ET.Element("robot") # create + root_xml.set("name", armature.name) + + root_xml.append(ET.Comment("LINKS")) + for l in links: + root_xml.append(l) + + root_xml.append(ET.Comment("JOINTS")) + for j in joints: + root_xml.append(j) + + # dump the xml string + ET_raw_string = ET.tostring(root_xml, encoding="unicode") + dom = minidom.parseString(ET_raw_string) + ET_pretty_string = dom.toprettyxml() + + with open(os.path.join(final_out_path, "armature.urdf"), "w") as f: + f.write(ET_pretty_string) + + return ET_pretty_string + + +if __name__ == "__main__": + # NOTE: this must be run from within Blender and by defaults saves viles in "blender_armatures/" relative to the directory containing the script + export_path = os.path.join( + os.path.dirname(bpy.context.space_data.text.filepath), "blender_armatures" + ) + # Optionally override the save directory with an absolute path of your choice + # export_path = "/home/my_path_choice/" + + # To use this script: + # 1. run with export meshes True + # 2. undo changes in the editor + # 3. run with export meshes False + # 4. undo changes in the editor + export_meshes = False + + urdf_str = export( + export_path, + { + "armature": get_armature(), + #'def_limit_effort': 100, #custom default effort limit for joints + #'def_limit_vel': 3, #custom default vel limit for joints + }, + export_meshes=export_meshes, + ) + print(f"\n ======== Output saved to {export_path} ========\n") From 702e5f527913b778cb6b0a99aa71d911e0f136a9 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 15 Aug 2023 08:25:32 -0700 Subject: [PATCH 087/153] turn off debug vis flags --- tools/blender_armature_to_urdf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 8565591eee..e0fa7a7574 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -186,7 +186,7 @@ def add_color_material_to_visual(color, xml_visual) -> None: xml_visual.append(this_xml_material) -def bone_to_urdf(this_bone, collision_visuals=True, joint_visual=True): +def bone_to_urdf(this_bone, collision_visuals=False, joint_visual=False): """This function extracts the basic properties of the bone and populates links and joint lists with the corresponding urdf nodes""" From f9c6744557f0eb4ac3970f67dee62142d37698be Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 16 Aug 2023 16:31:26 -0700 Subject: [PATCH 088/153] fix bugs introduced by flaker :( --- tools/blender_armature_to_urdf.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index e0fa7a7574..71b0fac9ef 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -473,11 +473,11 @@ def get_anim_limits_info(bone): is_prismatic = False is_revolute = False - for ac_key in bpy.data.actions: - if bone.name in ac_key: - key_match = [key for key in bone_limits if key in ac_key][0] + for ac in bpy.data.actions: + if bone.name in ac.name: + key_match = [key for key in bone_limits if key in ac.name][0] limit_list = [] - for _fkey, fcurve in bpy.data.actions[ac_key].fcurves.items(): + for _fkey, fcurve in ac.fcurves.items(): assert ( len(fcurve.keyframe_points) == 1 ), "Expecting one keyframe per track." @@ -549,7 +549,9 @@ def export(dirpath, settings, export_meshes: bool = True): joints = [] # get the armature - armature = settings.get("armature", bpy.data.objects["Armature"]) + armature = settings.get("armature") + if armature is None: + armature = bpy.data.objects["Armature"] # find the root bone root_bone = get_root_bone() From fbd1998bc2d8d0a7111b256fac196f73a9215535 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 16 Aug 2023 16:52:12 -0700 Subject: [PATCH 089/153] refactor to correct for rest pose and corrdinate change in prismatic joint axis also --- tools/blender_armature_to_urdf.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 71b0fac9ef..b30d4e70b9 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -373,7 +373,7 @@ def create_bone_link(this_bone): # create limits node limit_node = ET.fromstring(BASE_LIMIT_NODE_STR) - axis = Vector() + local_axis = Vector() # Revolute if len(bone_limits["lower_limit"]) == 4: @@ -382,14 +382,7 @@ def create_bone_link(this_bone): end = Quaternion(bone_limits["upper_limit"]) rest = Quaternion() diff = begin.rotation_difference(end) - axis, angle = diff.to_axis_angle() - # NOTE: rest pose could be applied to the bone resulting in an additional rotation stored in the matrix property - rest_correction = this_bone.matrix - # NOTE: Blender bones and armature are always Y-up, so we need to rotate the axis into URDF coordinate space (Z-up) - bone_axis = this_bone.vector - to_z_up = bone_axis.rotation_difference(Vector([0, 0, 1])) - # apply all rotations to arrive at the URDF Joint axis - axis = rest_correction @ (to_z_up @ axis) + local_axis, angle = diff.to_axis_angle() rest_diff = begin.rotation_difference(rest) rest_axis, rest_angle = rest_diff.to_axis_angle() limit_node.set("lower", f"{-rest_angle}") @@ -401,10 +394,18 @@ def create_bone_link(this_bone): upper_vec = Vector(bone_limits["upper_limit"]) lower_vec = Vector(bone_limits["lower_limit"]) displacement = upper_vec - lower_vec - axis = displacement.normalized() + local_axis = displacement.normalized() limit_node.set("lower", f"{-lower_vec.length}") limit_node.set("upper", f"{upper_vec.length}") + # NOTE: rest pose could be applied to the bone resulting in an additional rotation stored in the matrix property + rest_correction = this_bone.matrix + # NOTE: Blender bones and armature are always Y-up, so we need to rotate the axis into URDF coordinate space (Z-up) + bone_axis = this_bone.vector + to_z_up = bone_axis.rotation_difference(Vector([0, 0, 1])) + # apply all rotations to arrive at the URDF Joint axis + axis = rest_correction @ (to_z_up @ local_axis) + xml_axis = AXIS_NODE_FORMAT() xml_axis.set("xyz", ORIGIN_NODE_FORMAT.format(axis.x, axis.y, axis.z)) From cb93e12b163b78e6ade6396aea67e655719bd5b2 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 17 Aug 2023 11:44:54 -0700 Subject: [PATCH 090/153] export urdf with armature name --- tools/blender_armature_to_urdf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index b30d4e70b9..3a2bf42aaf 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -640,7 +640,7 @@ def export(dirpath, settings, export_meshes: bool = True): dom = minidom.parseString(ET_raw_string) ET_pretty_string = dom.toprettyxml() - with open(os.path.join(final_out_path, "armature.urdf"), "w") as f: + with open(os.path.join(final_out_path, f"{root_node.name}.urdf"), "w") as f: f.write(ET_pretty_string) return ET_pretty_string From 5cb05b9c6f5cee8d2b651679022d6b89e018db05 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 17 Aug 2023 16:55:47 -0700 Subject: [PATCH 091/153] add commandline arguements and instructions for the script --- tools/blender_armature_to_urdf.py | 133 +++++++++++++++++++++++++----- 1 file changed, 114 insertions(+), 19 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 3a2bf42aaf..adf6ec8ef9 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -118,13 +118,15 @@ def get_mesh_heirarchy( return selected_objects -def walk_armature(this_bone, handler): +def walk_armature(this_bone, handler, kwargs_for_handler=None): """ Recursively apply a handler function to bone children to traverse the armature. """ - handler(this_bone) + if kwargs_for_handler is None: + kwargs_for_handler = {} + handler(this_bone, **kwargs_for_handler) for child in this_bone.children: - walk_armature(child, handler) + walk_armature(child, handler, kwargs_for_handler) def bone_info(bone): @@ -186,10 +188,16 @@ def add_color_material_to_visual(color, xml_visual) -> None: xml_visual.append(this_xml_material) -def bone_to_urdf(this_bone, collision_visuals=False, joint_visual=False): +def bone_to_urdf( + this_bone, link_visuals=True, collision_visuals=False, joint_visuals=False +): """This function extracts the basic properties of the bone and populates links and joint lists with the corresponding urdf nodes""" + print(f"link_visuals = {link_visuals}") + print(f"collision_visuals = {collision_visuals}") + print(f"joint_visuals = {joint_visuals}") + global counter # Create the joint xml node @@ -227,11 +235,11 @@ def bone_to_urdf(this_bone, collision_visuals=False, joint_visual=False): # NOTE: we can use zero because we reset the origin for the meshes before exporting them to glb this_xml_visual.append(ZERO_ORIGIN_NODE()) this_xml_visual.append(this_xml_mesh_geom) - if not collision_visuals: + if link_visuals: this_xml_link.append(this_xml_visual) # NOTE: visual debugging tool to add a box at the joint pivot locations - if joint_visual: + if joint_visuals: this_xml_visual = ET.Element("visual") this_xml_test_geom = ET.Element("geometry") this_xml_box = ET.Element("box") @@ -537,13 +545,17 @@ def get_armature(): return None -def export(dirpath, settings, export_meshes: bool = True): +def export(dirpath, settings, export_meshes: bool = True, **kwargs): """ Run the Armature to URDF converter and export the .urdf file. Recursively travserses the armature bone tree and constructs Links and Joints. Note: This process is destructive and requires undo or revert in the editor after use. + + :return: export directory or URDF filepath """ + output_path = dirpath + global LINK_NAME_FORMAT, JOINT_NAME_FORMAT, armature, root_bone, links, joints, counter counter = 0 links = [] @@ -610,7 +622,7 @@ def export(dirpath, settings, export_meshes: bool = True): use_selection=True, export_yup=False, ) - return None + return output_path # print("------------------------") # print("Bone info recursion:") @@ -621,7 +633,7 @@ def export(dirpath, settings, export_meshes: bool = True): # print("------------------------") # Recursively generate the xml elements - walk_armature(root_bone, bone_to_urdf) + walk_armature(root_bone, bone_to_urdf, kwargs_for_handler=kwargs) # add all the joints and links to the root root_xml = ET.Element("robot") # create @@ -640,28 +652,108 @@ def export(dirpath, settings, export_meshes: bool = True): dom = minidom.parseString(ET_raw_string) ET_pretty_string = dom.toprettyxml() - with open(os.path.join(final_out_path, f"{root_node.name}.urdf"), "w") as f: + output_path = os.path.join(final_out_path, f"{root_node.name}.urdf") + + with open(output_path, "w") as f: f.write(ET_pretty_string) - return ET_pretty_string + return output_path if __name__ == "__main__": # NOTE: this must be run from within Blender and by defaults saves viles in "blender_armatures/" relative to the directory containing the script - export_path = os.path.join( - os.path.dirname(bpy.context.space_data.text.filepath), "blender_armatures" - ) - # Optionally override the save directory with an absolute path of your choice - # export_path = "/home/my_path_choice/" - # To use this script: + export_path = None + try: + os.path.join( + os.path.dirname(bpy.context.space_data.text.filepath), "blender_armatures" + ) + except BaseException: + print( + "Couldn't get the directory from the filepath. E.g. running from commandline." + ) + + # ----------------------------------------------------------- + # To use this script in Blender editor: # 1. run with export meshes True # 2. undo changes in the editor # 3. run with export meshes False # 4. undo changes in the editor + + # NOTE: the following settings are overridden by commandline arguments if provided + + # Optionally override the save directory with an absolute path of your choice + # export_path = "/home/my_path_choice/" + export_meshes = False - urdf_str = export( + # visual shape export flags for debugging + link_visuals = True + collision_visuals = False + joint_visuals = False + # ----------------------------------------------------------- + + # ----------------------------------------------------------- + # To use from the commandline: + # 1. `blender .blend --background --python blender_armature_to_urdf.py -- --export-path + # 2. add `--export-meshes` to export the link .glbs + # Note: ' -- ' tells Blender to ignore the remaining arguemnts, so we pass anything after that into the script arguements below: + import sys + + argv = sys.argv + py_argv = "" + if "--" in argv: + py_argv = argv[argv.index("--") + 1 :] # get all args after "--" + + import argparse + + parser = argparse.ArgumentParser() + + parser.add_argument( + "--export-path", + default=export_path, + type=str, + help="Path to the output directory for meshes and URDF.", + ) + parser.add_argument( + "--export-meshes", + action="store_true", + default=export_meshes, + help="Export meshes for the link objects. If not set, instead generate the URDF.", + ) + + # Debugging flags: + parser.add_argument( + "--no-link-visuals", + action="store_true", + default=not link_visuals, + help="Don't include visual mesh shapes in the exported URDF. E.g. for debugging.", + ) + parser.add_argument( + "--collision-visuals", + action="store_true", + default=collision_visuals, + help="Include visual shapes for collision primitives in the exported URDF. E.g. for debugging.", + ) + parser.add_argument( + "--joint-visuals", + action="store_true", + default=joint_visuals, + help="Include visual box shapes for joint pivot locations in the exported URDF. E.g. for debugging.", + ) + + args = parser.parse_args(py_argv) + + export_meshes = args.export_meshes + export_path = args.export_path + + # ----------------------------------------------------------- + + assert ( + export_path is not None + ), "No export path provided. If running from commandline, provide a path with '--export-path ' after ' -- '." + + output_path = export( export_path, { "armature": get_armature(), @@ -669,5 +761,8 @@ def export(dirpath, settings, export_meshes: bool = True): #'def_limit_vel': 3, #custom default vel limit for joints }, export_meshes=export_meshes, + link_visuals=not args.no_link_visuals, + collision_visuals=args.collision_visuals, + joint_visuals=args.joint_visuals, ) - print(f"\n ======== Output saved to {export_path} ========\n") + print(f"\n ======== Output saved to {output_path} ========\n") From 9610cefe9a6ce7b9cdc3522a27275d0e09594606 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 27 Sep 2023 11:46:15 -0700 Subject: [PATCH 092/153] add receptacle classification for meshes --- tools/blender_armature_to_urdf.py | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index adf6ec8ef9..a337aac0f3 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -83,27 +83,38 @@ def is_collision_mesh(mesh_obj) -> bool: return "collision" in mesh_obj.name +def is_receptacle_mesh(mesh_obj) -> bool: + """ + Is the object a receptacle mesh. + """ + return "receptacle" in mesh_obj.name + + def get_mesh_heirarchy( mesh_obj, select_set: bool = True, - include_non_collision: bool = True, + include_render: bool = True, include_collison: bool = False, + include_receptacle: bool = False, ) -> List[Any]: """ Select all MESH objects in the heirarchy specifically targeting or omitting meshes with "collision" in the name. :param mesh_obj: The Blender mesh object. :param select_set: Whether or not to select the objects as well as recording them. - :param include_non_collision: Include objects without 'collision' in the name. + :param include_render: Include render objects without qualifiers in the name. :param include_collison: Include objects with 'collision' in the name. + :param include_receptacle: Include objects with 'receptacle' in the name. :return: The list of Blender mesh objects. """ selected_objects = [] is_col_mesh = is_collision_mesh(mesh_obj) + is_rec_mesh = is_receptacle_mesh(mesh_obj) if is_mesh(mesh_obj) and ( (is_col_mesh and include_collison) - or (include_non_collision and not is_col_mesh) + and (is_rec_mesh and include_receptacle) + or (include_render and not is_col_mesh) ): selected_objects.append(mesh_obj) if select_set: @@ -112,7 +123,11 @@ def get_mesh_heirarchy( if child.type != "ARMATURE": selected_objects.extend( get_mesh_heirarchy( - child, select_set, include_non_collision, include_collison + child, + select_set, + include_render, + include_collison, + include_receptacle, ) ) return selected_objects @@ -220,7 +235,7 @@ def bone_to_urdf( mesh_obj, select_set=False, include_collison=True, - include_non_collision=False, + include_render=False, ) ) if mesh_obj.parent is not None and mesh_obj.parent.type == "ARMATURE": From 42e7dcacbb9b304bcee3bed0be077980e91b8d35 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 3 Oct 2023 15:58:23 -0700 Subject: [PATCH 093/153] exporting receptacle meshes --- tools/blender_armature_to_urdf.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index a337aac0f3..a516a6547d 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -113,7 +113,7 @@ def get_mesh_heirarchy( is_rec_mesh = is_receptacle_mesh(mesh_obj) if is_mesh(mesh_obj) and ( (is_col_mesh and include_collison) - and (is_rec_mesh and include_receptacle) + or (is_rec_mesh and include_receptacle) or (include_render and not is_col_mesh) ): selected_objects.append(mesh_obj) @@ -603,6 +603,7 @@ def export(dirpath, settings, export_meshes: bool = True, **kwargs): # print all mesh object parents, reset origins for mesh export and transformation registery, collect bone to mesh map root_node = None + receptacle_meshes = [] for obj in bpy.data.objects: if obj.type == "MESH": parent_bone = get_parent_bone(obj) @@ -613,6 +614,8 @@ def export(dirpath, settings, export_meshes: bool = True, **kwargs): if obj.parent_bone != "": bones_to_meshes[obj.parent_bone].append(obj) print(f" -pb> {obj.parent_bone}") + if is_receptacle_mesh(obj): + receptacle_meshes.append(obj) elif obj.type == "EMPTY": print(f"EMPTY: {obj.name}") if obj.parent is None and len(obj.children) > 0: @@ -637,6 +640,19 @@ def export(dirpath, settings, export_meshes: bool = True, **kwargs): use_selection=True, export_yup=False, ) + # export receptacle meshes + for rec_mesh in receptacle_meshes: + clear_obj_transform(rec_mesh) + deselect_all() + rec_mesh.select_set(True) + bpy.ops.export_scene.gltf( + filepath=os.path.join( + final_out_path, rec_mesh.parent.name + "_receptacle" + ), + use_selection=True, + export_yup=False, + ) + return output_path # print("------------------------") From 63ead643e6964ee37cce7e31660b51eb20a88e15 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 19 Oct 2023 12:23:24 -0700 Subject: [PATCH 094/153] add batched converter script --- tools/batched_armature_to_urdf.py | 114 ++++++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) create mode 100644 tools/batched_armature_to_urdf.py diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py new file mode 100644 index 0000000000..173b8a8038 --- /dev/null +++ b/tools/batched_armature_to_urdf.py @@ -0,0 +1,114 @@ +# 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 argparse +import os +from typing import Callable, List + + +def file_endswith(filepath: str, end_str: str) -> bool: + """ + Return whether or not the file ends with a string. + """ + return filepath.endswith(end_str) + + +def find_files( + root_dir: str, discriminator: Callable[[str, str], bool], disc_str: str +) -> List[str]: + """ + Recursively find all filepaths under a root directory satisfying a particular constraint as defined by a discriminator function. + + :param root_dir: The roor directory for the recursive search. + :param discriminator: The discriminator function which takes a filepath and discriminator string and returns a bool. + + :return: The list of all absolute filepaths found satisfying the discriminator. + """ + filepaths: List[str] = [] + + if not os.path.exists(root_dir): + print(" Directory does not exist: " + str(dir)) + return filepaths + + for entry in os.listdir(root_dir): + entry_path = os.path.join(root_dir, entry) + if os.path.isdir(entry_path): + sub_dir_filepaths = find_files(entry_path, discriminator, disc_str) + filepaths.extend(sub_dir_filepaths) + # apply a user-provided discriminator function to cull filepaths + elif discriminator(entry_path, disc_str): + filepaths.append(entry_path) + return filepaths + + +def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: str): + assert os.path.exists(blend_file), f"'{blend_file}' does not exist." + os.makedirs(export_path, exist_ok=True) + base_command = f"blender {blend_file} --background --python {script_path} -- --export-path {export_path}" + # first export the meshes + os.system(base_command + " --export-meshes") + # then export the URDF + os.system(base_command) + + +# ----------------------------------------- +# Batches blender converter calls over a directory of blend files +# e.g. python tools/batched_armature_to_urdf.py --root-dir ~/Downloads/OneDrive_1_9-27-2023/ --out-dir tools/armature_out_test/ --converter-script-path tools/blender_armature_to_urdf.py +# e.g. add " --skip-strings wardrobe" to skip all objects with "wardrobe" in the filepath +# ----------------------------------------- +def main(): + parser = argparse.ArgumentParser( + description="Run Blender Armature to URDF converter for all .blend files in a directory." + ) + parser.add_argument( + "--root-dir", + type=str, + help="Path to a directory containing .blend files for conversion.", + ) + parser.add_argument( + "--out-dir", + type=str, + help="Path to a directory for exporting URDF and assets.", + ) + parser.add_argument( + "--converter-script-path", + type=str, + help="Path to blender_armature_to_urdf.py.", + default="tools/blender_armature_to_urdf.py", + ) + parser.add_argument( + "--skip-strings", + nargs="+", + type=str, + help="Substrings which indicate a path which should be skippped. E.g. an object hash '6f57e5076e491f54896631bfe4e9cfcaa08899e2' to skip that object's blend file.", + default=None, + ) + args = parser.parse_args() + root_dir = args.root_dir + assert os.path.isdir(root_dir), "directory must exist." + assert os.path.exists( + args.converter_script_path + ), f"provided script path '{args.converter_script_path}' does not exist." + + # get blend files + blend_paths = find_files(root_dir, file_endswith, ".blend") + if args.skip_strings is not None: + skipped_strings = [ + path + for skip_str in args.skip_strings + for path in blend_paths + if skip_str in path + ] + blend_paths = list(set(blend_paths) - set(skipped_strings)) + + for blend_path in blend_paths: + run_armature_urdf_conversion( + blend_file=blend_path, + export_path=args.out_dir, + script_path=args.converter_script_path, + ) + + +if __name__ == "__main__": + main() From a48867387d0754cb2a6f9b1437dad8307ce075e2 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 23 Oct 2023 12:59:48 -0700 Subject: [PATCH 095/153] use epsilon error instead of equality comparison for scale floats --- tools/blender_armature_to_urdf.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index a516a6547d..64ff57d65c 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -292,15 +292,18 @@ def bone_to_urdf( box_size = col.scale this_xml_box.set("size", f"{box_size.x} {box_size.y} {box_size.z}") xml_shape = this_xml_box - if "collision_cylinder" in col.name: + elif "collision_cylinder" in col.name: this_xml_cyl = ET.Element("cylinder") scale = col.scale # radius XY axis scale must match - assert scale.x == scale.y, "XY dimensions must match. Used as radius." + assert ( + abs(scale.x - scale.y) < 0.0001 + ), f"XY dimensions must match. Used as radius. node_name=='{col.name}', x={scale.x}, y={scale.y}" this_xml_cyl.set("radius", f"{scale.x/2.0}") # NOTE: assume Z axis is length of the cylinder this_xml_cyl.set("length", f"{scale.z}") xml_shape = this_xml_cyl + this_xml_col_geom.append(xml_shape) # first get the rotation xml_origin = get_origin_from_matrix(col.matrix_local) From ca90919add5196473634c736d8291a2f0b9ec3f2 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 23 Oct 2023 13:26:52 -0700 Subject: [PATCH 096/153] add support for collision spheres --- tools/blender_armature_to_urdf.py | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 64ff57d65c..e1bc8a2147 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -267,10 +267,23 @@ def bone_to_urdf( # NOTE: color each link's collision shapes for debugging this_color = get_next_color() + supported_collision_shapes = [ + "collision_box", + "collision_cylinder", + "collision_sphere", + ] + for col in collision_objects: assert ( - "collision_box" in col.name or "collision_cylinder" in col.name - ), "Only supporting collision boxes and cylinders currently." + len( + [ + col_name + for col_name in supported_collision_shapes + if col_name in col.name + ] + ) + == 1 + ), f"Only supporting exactly one of the following collision shapes currently: {supported_collision_shapes}. Shape name '{col.name}' unsupported." set_obj_origin_to_center(col) clear_obj_transform( @@ -303,6 +316,15 @@ def bone_to_urdf( # NOTE: assume Z axis is length of the cylinder this_xml_cyl.set("length", f"{scale.z}") xml_shape = this_xml_cyl + elif "collision_sphere" in col.name: + this_xml_sphere = ET.Element("sphere") + scale = col.scale + # radius XYZ axis scale must match + assert ( + abs(scale.x - scale.y) < 0.0001 and abs(scale.x - scale.z) < 0.0001 + ), f"XYZ dimensions must match. Used as radius. node_name=='{col.name}', x={scale.x}, y={scale.y}, z={scale.z}" + this_xml_sphere.set("radius", f"{scale.x/2.0}") + xml_shape = this_xml_sphere this_xml_col_geom.append(xml_shape) # first get the rotation From 12ead8d1dbd62184d061f587545ae5d2ed5c58ba Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 2 Nov 2023 13:27:28 -0700 Subject: [PATCH 097/153] add ao_config export and scne limited batching --- tools/batched_armature_to_urdf.py | 53 +++++++++++++++++++++++++++++-- tools/blender_armature_to_urdf.py | 31 +++++++++++++++++- 2 files changed, 81 insertions(+), 3 deletions(-) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index 173b8a8038..309785d4a5 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -3,8 +3,10 @@ # LICENSE file in the root directory of this source tree. import argparse +import csv import os -from typing import Callable, List +from collections import defaultdict +from typing import Callable, Dict, List def file_endswith(filepath: str, end_str: str) -> bool: @@ -42,6 +44,29 @@ def find_files( return filepaths +def load_scene_map_file(filepath: str) -> Dict[str, List[str]]: + """ + Loads a csv file containing a mapping of scenes to objects. Returns that mapping as a Dict. + NOTE: Assumes column 0 is object id and column 1 is scene id + """ + assert os.path.exists(filepath) + assert filepath.endswith(".csv") + + scene_object_map = defaultdict(lambda: []) + with open(filepath, newline="") as f: + reader = csv.reader(f) + for rix, row in enumerate(reader): + if rix == 0: + pass + # column labels + else: + scene_id = row[1] + object_hash = row[0] + scene_object_map[scene_id].append(object_hash) + + return scene_object_map + + def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: str): assert os.path.exists(blend_file), f"'{blend_file}' does not exist." os.makedirs(export_path, exist_ok=True) @@ -49,7 +74,7 @@ def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: # first export the meshes os.system(base_command + " --export-meshes") # then export the URDF - os.system(base_command) + os.system(base_command + " -- export-ao-config") # ----------------------------------------- @@ -84,6 +109,19 @@ def main(): help="Substrings which indicate a path which should be skippped. E.g. an object hash '6f57e5076e491f54896631bfe4e9cfcaa08899e2' to skip that object's blend file.", default=None, ) + parser.add_argument( + "--scene-map-file", + type=str, + default=None, + help="Path to a csv file with scene to object mappings. Used in conjuction with 'scenes' to limit conversion to a small batch.", + ) + parser.add_argument( + "--scenes", + nargs="+", + type=str, + help="Substrings which indicate scenes which should be converted. Must be provided with a scene map file. When provided, only these scenes are converted.", + default=None, + ) args = parser.parse_args() root_dir = args.root_dir assert os.path.isdir(root_dir), "directory must exist." @@ -102,6 +140,17 @@ def main(): ] blend_paths = list(set(blend_paths) - set(skipped_strings)) + if args.scene_map_file is not None and args.scenes is not None: + # load the scene map file and limit the object set by scenes + scene_object_map = load_scene_map_file(args.scene_map_file) + limited_object_paths = [] + for scene in args.scenes: + for object_id in scene_object_map[scene]: + for blend_path in blend_paths: + if object_id in blend_path: + limited_object_paths.append(blend_path) + blend_paths = list(set(limited_object_paths)) + for blend_path in blend_paths: run_armature_urdf_conversion( blend_file=blend_path, diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index e1bc8a2147..a6658c4e0c 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -2,6 +2,7 @@ # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. +import json import os import xml.dom.minidom as minidom import xml.etree.ElementTree as ET @@ -585,7 +586,13 @@ def get_armature(): return None -def export(dirpath, settings, export_meshes: bool = True, **kwargs): +def export( + dirpath, + settings, + export_meshes: bool = True, + export_ao_config: bool = True, + **kwargs, +): """ Run the Armature to URDF converter and export the .urdf file. Recursively travserses the armature bone tree and constructs Links and Joints. @@ -713,6 +720,20 @@ def export(dirpath, settings, export_meshes: bool = True, **kwargs): with open(output_path, "w") as f: f.write(ET_pretty_string) + if export_ao_config: + # write the ao_config + ao_config_contents = { + "urdf_filepath": f"{root_node.name}.urdf", + "user_defined": { + # insert receptacle metadata here + }, + } + ao_config_filename = os.path.join( + final_out_path, f"{root_node.name}.ao_config.json" + ) + with open(ao_config_filename, "w") as f: + f.write(json.dump(ao_config_contents, f)) + return output_path @@ -742,6 +763,7 @@ def export(dirpath, settings, export_meshes: bool = True, **kwargs): # export_path = "/home/my_path_choice/" export_meshes = False + export_ao_config = False # visual shape export flags for debugging link_visuals = True @@ -777,6 +799,12 @@ def export(dirpath, settings, export_meshes: bool = True, **kwargs): default=export_meshes, help="Export meshes for the link objects. If not set, instead generate the URDF.", ) + parser.add_argument( + "--export-ao-config", + action="store_true", + default=export_ao_config, + help="Export a *.ao_config.json file for the URDF.", + ) # Debugging flags: parser.add_argument( @@ -817,6 +845,7 @@ def export(dirpath, settings, export_meshes: bool = True, **kwargs): #'def_limit_vel': 3, #custom default vel limit for joints }, export_meshes=export_meshes, + export_ao_config=export_ao_config, link_visuals=not args.no_link_visuals, collision_visuals=args.collision_visuals, joint_visuals=args.joint_visuals, From c20a4e45402139188f61e7c39b4445a72ddf7147 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 2 Nov 2023 14:05:29 -0700 Subject: [PATCH 098/153] export full receptacle information with config --- tools/batched_armature_to_urdf.py | 2 +- tools/blender_armature_to_urdf.py | 16 ++++++++++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index 309785d4a5..76ba9b5d71 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -74,7 +74,7 @@ def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: # first export the meshes os.system(base_command + " --export-meshes") # then export the URDF - os.system(base_command + " -- export-ao-config") + os.system(base_command + " --export-ao-config") # ----------------------------------------- diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index a6658c4e0c..04cfdd2c78 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -636,6 +636,7 @@ def export( # print all mesh object parents, reset origins for mesh export and transformation registery, collect bone to mesh map root_node = None receptacle_meshes = [] + receptacle_to_link_name = {} for obj in bpy.data.objects: if obj.type == "MESH": parent_bone = get_parent_bone(obj) @@ -648,6 +649,9 @@ def export( print(f" -pb> {obj.parent_bone}") if is_receptacle_mesh(obj): receptacle_meshes.append(obj) + receptacle_to_link_name[ + obj.parent.name + "_receptacle" + ] = obj.parent.name elif obj.type == "EMPTY": print(f"EMPTY: {obj.name}") if obj.parent is None and len(obj.children) > 0: @@ -728,6 +732,17 @@ def export( # insert receptacle metadata here }, } + for rec_name, link_name in receptacle_to_link_name.items(): + ao_config_contents["user_defined"][rec_name] = { + "name": rec_name, + "parent_object": f"{root_node.name}", + "parent_link": link_name, + "position": [0, 0, 0], + "rotation": [1, 0, 0, 0], + "scale": [1, 1, 1], + "up": [0, 1, 0], + "mesh_filepath": rec_name + ".glb", + } ao_config_filename = os.path.join( final_out_path, f"{root_node.name}.ao_config.json" ) @@ -829,6 +844,7 @@ def export( args = parser.parse_args(py_argv) export_meshes = args.export_meshes + export_ao_config = args.export_ao_config export_path = args.export_path # ----------------------------------------------------------- From 592fbe27e3fcafb2191f5e1d712229d1cf3d5d43 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 2 Nov 2023 14:51:41 -0700 Subject: [PATCH 099/153] first draft of scene-level script to replace rigid objects with articulated objects --- ...place_articulated_models_in_rigid_scene.py | 159 ++++++++++++++++++ 1 file changed, 159 insertions(+) create mode 100644 tools/replace_articulated_models_in_rigid_scene.py diff --git a/tools/replace_articulated_models_in_rigid_scene.py b/tools/replace_articulated_models_in_rigid_scene.py new file mode 100644 index 0000000000..421ccbf7d0 --- /dev/null +++ b/tools/replace_articulated_models_in_rigid_scene.py @@ -0,0 +1,159 @@ +# 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 argparse +import json +import os +from typing import Callable, List + + +def file_is_scene_config(filepath: str) -> bool: + """ + Return whether or not the file is an scene_instance.json + """ + return filepath.endswith(".scene_instance.json") + + +def file_is_urdf(filepath: str) -> bool: + """ + Return whether or not the file is a .urdf + """ + return filepath.endswith(".urdf") + + +def find_files(root_dir: str, discriminator: Callable[[str], bool]) -> List[str]: + """ + Recursively find all filepaths under a root directory satisfying a particular constraint as defined by a discriminator function. + + :param root_dir: The roor directory for the recursive search. + :param discriminator: The discriminator function which takes a filepath and returns a bool. + + :return: The list of all absolute filepaths found satisfying the discriminator. + """ + filepaths: List[str] = [] + + if not os.path.exists(root_dir): + print(" Directory does not exist: " + str(dir)) + return filepaths + + for entry in os.listdir(root_dir): + entry_path = os.path.join(root_dir, entry) + if os.path.isdir(entry_path): + sub_dir_filepaths = find_files(entry_path, discriminator) + filepaths.extend(sub_dir_filepaths) + # apply a user-provided discriminator function to cull filepaths + elif discriminator(entry_path): + filepaths.append(entry_path) + return filepaths + + +scenes_without_filters = [] + + +def find_and_replace_articulated_models_for_config( + filepath: str, top_dir: str, urdf_names: str +) -> None: + """ + For a given scene config, try to find a matching articulated objects for each rigid object. If found, add them to the config, replacing the rigid objects. + + :param top_dir: The top directory of the dataset from which the rec filter file path should be relative. + """ + assert filepath.endswith(".scene_instance.json"), "Must be a scene instance JSON." + scene_name = filepath.split(".scene_instance.json")[0].split("/")[-1] + + print(f"scene_name = {scene_name}") + + file_is_modified = False + with open(filepath, "r") as f: + scene_conf = json.load(f) + + ao_instance_data = [] + if "articulated_object_instances" in scene_conf: + ao_instance_data = scene_conf["articulated_object_instances"] + + modified_object_instance_data = [] + for object_instance_data in scene_conf["object_instances"]: + object_name = object_instance_data["template_name"] + + # look for a matching articulated object entry + urdf_name_match = None + for urdf_name in urdf_names: + if object_name in urdf_name: + urdf_name_match = urdf_name + break + + # create the modified JSON data + if urdf_name_match is None: + # add the object to the modified list + modified_object_instance_data.append(object_instance_data) + else: + file_is_modified = True + # TODO: all objects are non-uniformly scaled and won't fit exactly in the scenes... + # assert "non_uniform_scale" not in object_instance_data, "Rigid object is non-uniformaly scaled. Cannot replace with equivalent articulated object." + this_ao_instance_data = { + "template_name": urdf_name_match, + "translation_origin": "COM", + "fixed_base": True, + "motion_type": "DYNAMIC", + } + if "translation" in object_instance_data: + this_ao_instance_data["translation"] = object_instance_data[ + "translation" + ] + if "rotation" in object_instance_data: + this_ao_instance_data["rotation"] = object_instance_data[ + "translation" + ] + ao_instance_data.append(this_ao_instance_data) + + scene_conf["object_instances"] = modified_object_instance_data + + if file_is_modified: + with open(filepath, "w") as f: + json.dump(scene_conf, f) + + +def main(): + parser = argparse.ArgumentParser( + description="Add user_defined entry to scene_instance.json files referencing the rec_filter_file." + ) + parser.add_argument( + "--dataset-root-dir", + type=str, + help="path to HSSD SceneDataset root directory containing 'fphab-uncluttered.scene_dataset_config.json'.", + ) + parser.add_argument( + "--scenes", + nargs="+", + type=str, + help="Substrings which indicate scenes which should be converted. When provided, only these scenes are converted.", + default=None, + ) + args = parser.parse_args() + fp_root_dir = args.dataset_root_dir + config_root_dir = os.path.join(fp_root_dir, "scenes-uncluttered") + configs = find_files(config_root_dir, file_is_scene_config) + urdf_dir = os.path.join(fp_root_dir, "urdf/") + urdf_files = find_files(urdf_dir, file_is_urdf) + urdf_names = [ + urdf_filename.split("/")[-1].split(".urdf")[0] for urdf_filename in urdf_files + ] + + # limit the scenes which are converted + if args.scenes is not None: + scene_limited_configs = [] + for scene in args.scenes: + for config in configs: + if scene + "." in config: + scene_limited_configs.append(config) + configs = list(set(scene_limited_configs)) + + for _ix, filepath in enumerate(configs): + find_and_replace_articulated_models_for_config( + filepath, urdf_names=urdf_names, top_dir=fp_root_dir + ) + + +if __name__ == "__main__": + main() From 3593ad64c76154df17ae1647a838eb9b4cc178b0 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 3 Nov 2023 10:31:29 -0700 Subject: [PATCH 100/153] add root rotation correction to converter --- tools/batched_armature_to_urdf.py | 2 +- tools/blender_armature_to_urdf.py | 35 ++++++++++++++++++++++++++++++- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index 76ba9b5d71..b481e7ecc8 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -72,7 +72,7 @@ def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: os.makedirs(export_path, exist_ok=True) base_command = f"blender {blend_file} --background --python {script_path} -- --export-path {export_path}" # first export the meshes - os.system(base_command + " --export-meshes") + # os.system(base_command + " --export-meshes") # then export the URDF os.system(base_command + " --export-ao-config") diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 04cfdd2c78..7f28ec0cc6 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -3,6 +3,7 @@ # LICENSE file in the root directory of this source tree. import json +import math import os import xml.dom.minidom as minidom import xml.etree.ElementTree as ET @@ -10,7 +11,7 @@ from typing import Any, Dict, List, Tuple import bpy -from mathutils import Quaternion, Vector +from mathutils import Matrix, Quaternion, Vector # Colors from https://colorbrewer2.org/ colors = [ @@ -586,6 +587,31 @@ def get_armature(): return None +def construct_root_rotation_joint(root_node_name): + """ + Construct the root rotation joint XML Element. + """ + xml_root_joint = ET.Element("joint") + xml_root_joint.set("name", "root_rotation") + xml_root_joint.set("type", "fixed") + + # construct a standard rotation matrix transform to apply to all root nodes + M = Matrix.Rotation(math.radians(-90.0), 4, "X") + xml_root_joint.append(get_origin_from_matrix(M)) + + # create parent node + parent_xml_node = ET.Element("parent") + parent_xml_node.set("link", "root") + + # create child node + child_xml_node = ET.Element("child") + child_xml_node.set("link", root_node_name) + + xml_root_joint.append(parent_xml_node) + xml_root_joint.append(child_xml_node) + return xml_root_joint + + def export( dirpath, settings, @@ -706,6 +732,13 @@ def export( root_xml = ET.Element("robot") # create root_xml.set("name", armature.name) + # add a coordinate change in a dummy root node + xml_root_link = ET.Element("link") + xml_root_link.set("name", "root") + xml_root_joint = construct_root_rotation_joint(root_bone.name) + root_xml.append(xml_root_link) + root_xml.append(xml_root_joint) + root_xml.append(ET.Comment("LINKS")) for l in links: root_xml.append(l) From 2f64fa5901f70216b108fdd6aa4d13f04b555588 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 3 Nov 2023 12:01:27 -0700 Subject: [PATCH 101/153] expose ArticulatedObject creation_attributes --- src/esp/physics/ArticulatedObject.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/esp/physics/ArticulatedObject.h b/src/esp/physics/ArticulatedObject.h index 43d44362a6..90ddf74fa7 100644 --- a/src/esp/physics/ArticulatedObject.h +++ b/src/esp/physics/ArticulatedObject.h @@ -896,6 +896,12 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { //! Cache the global scaling from the source model. Set during import. float globalScale_ = 1.0; + /** + * @brief Saved attributes when the object was initialized. + */ + metadata::attributes::ArticulatedObjectAttributes::ptr + initializationAttributes_ = nullptr; + public: ESP_SMART_POINTERS(ArticulatedObject) }; From d0eccdeb1c59efa8ec0922f50b492b40e7a1e633 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 3 Nov 2023 12:36:03 -0700 Subject: [PATCH 102/153] bugfixes --- tools/batched_armature_to_urdf.py | 2 +- tools/blender_armature_to_urdf.py | 7 ++++--- tools/replace_articulated_models_in_rigid_scene.py | 4 +--- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index b481e7ecc8..76ba9b5d71 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -72,7 +72,7 @@ def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: os.makedirs(export_path, exist_ok=True) base_command = f"blender {blend_file} --background --python {script_path} -- --export-path {export_path}" # first export the meshes - # os.system(base_command + " --export-meshes") + os.system(base_command + " --export-meshes") # then export the URDF os.system(base_command + " --export-ao-config") diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 7f28ec0cc6..deefdea153 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -116,7 +116,7 @@ def get_mesh_heirarchy( if is_mesh(mesh_obj) and ( (is_col_mesh and include_collison) or (is_rec_mesh and include_receptacle) - or (include_render and not is_col_mesh) + or (include_render and not is_col_mesh and not is_rec_mesh) ): selected_objects.append(mesh_obj) if select_set: @@ -704,7 +704,7 @@ def export( ) # export receptacle meshes for rec_mesh in receptacle_meshes: - clear_obj_transform(rec_mesh) + clear_obj_transform(rec_mesh.parent) deselect_all() rec_mesh.select_set(True) bpy.ops.export_scene.gltf( @@ -766,7 +766,8 @@ def export( }, } for rec_name, link_name in receptacle_to_link_name.items(): - ao_config_contents["user_defined"][rec_name] = { + rec_label = "receptacle_mesh_" + rec_name + ao_config_contents["user_defined"][rec_label] = { "name": rec_name, "parent_object": f"{root_node.name}", "parent_link": link_name, diff --git a/tools/replace_articulated_models_in_rigid_scene.py b/tools/replace_articulated_models_in_rigid_scene.py index 421ccbf7d0..470578bfeb 100644 --- a/tools/replace_articulated_models_in_rigid_scene.py +++ b/tools/replace_articulated_models_in_rigid_scene.py @@ -102,9 +102,7 @@ def find_and_replace_articulated_models_for_config( "translation" ] if "rotation" in object_instance_data: - this_ao_instance_data["rotation"] = object_instance_data[ - "translation" - ] + this_ao_instance_data["rotation"] = object_instance_data["rotation"] ao_instance_data.append(this_ao_instance_data) scene_conf["object_instances"] = modified_object_instance_data From f86470845cb4acb5539c45a00e1b5148aee0883a Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 6 Nov 2023 08:43:00 -0800 Subject: [PATCH 103/153] bugfix --- tools/replace_articulated_models_in_rigid_scene.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/tools/replace_articulated_models_in_rigid_scene.py b/tools/replace_articulated_models_in_rigid_scene.py index 470578bfeb..b1719af38e 100644 --- a/tools/replace_articulated_models_in_rigid_scene.py +++ b/tools/replace_articulated_models_in_rigid_scene.py @@ -106,8 +106,14 @@ def find_and_replace_articulated_models_for_config( ao_instance_data.append(this_ao_instance_data) scene_conf["object_instances"] = modified_object_instance_data + scene_conf["articulated_object_instances"] = ao_instance_data if file_is_modified: + filepath = ( + filepath.split("scenes-uncluttered")[0] + + "scenes-articulated-uncluttered" + + filepath.split("scenes-uncluttered")[-1] + ) with open(filepath, "w") as f: json.dump(scene_conf, f) From baa0a095b6c18e527337f5d1669e43e5788fc503 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 28 Nov 2023 16:58:37 -0800 Subject: [PATCH 104/153] adds a report generator for checking the output of a converted batch of files --- tools/generate_blend_to_urdf_parser_report.py | 209 ++++++++++++++++++ 1 file changed, 209 insertions(+) create mode 100644 tools/generate_blend_to_urdf_parser_report.py diff --git a/tools/generate_blend_to_urdf_parser_report.py b/tools/generate_blend_to_urdf_parser_report.py new file mode 100644 index 0000000000..b3bc17c505 --- /dev/null +++ b/tools/generate_blend_to_urdf_parser_report.py @@ -0,0 +1,209 @@ +# 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 argparse +import csv +import os +from typing import Callable, Dict, List + + +def file_endswith(filepath: str, end_str: str) -> bool: + """ + Return whether or not the file ends with a string. + """ + return filepath.endswith(end_str) + + +def find_files( + root_dir: str, discriminator: Callable[[str, str], bool], disc_str: str +) -> List[str]: + """ + Recursively find all filepaths under a root directory satisfying a particular constraint as defined by a discriminator function. + + :param root_dir: The roor directory for the recursive search. + :param discriminator: The discriminator function which takes a filepath and discriminator string and returns a bool. + + :return: The list of all absolute filepaths found satisfying the discriminator. + """ + filepaths: List[str] = [] + + if not os.path.exists(root_dir): + print(" Directory does not exist: " + str(root_dir)) + return filepaths + + for entry in os.listdir(root_dir): + entry_path = os.path.join(root_dir, entry) + if os.path.isdir(entry_path): + sub_dir_filepaths = find_files(entry_path, discriminator, disc_str) + filepaths.extend(sub_dir_filepaths) + # apply a user-provided discriminator function to cull filepaths + elif discriminator(entry_path, disc_str): + filepaths.append(entry_path) + return filepaths + + +def find_subdirectory_names(root_dir: str) -> List[str]: + """ + Lists all immediate child directories for a provided root directory. + """ + assert os.path.exists(root_dir) + + dirpaths = [] + + for entry in os.listdir(root_dir): + entry_path = os.path.join(root_dir, entry) + if os.path.isdir(entry_path): + dirpaths.append(entry) + + return dirpaths + + +def load_model_list_from_csv( + filepath: str, header_label: str = "Model ID" +) -> List[str]: + """ + Scrape a csv file for a list of model ids under a header label. + """ + assert filepath.endswith(".csv"), "This isn't a .csv file." + assert os.path.exists(filepath) + ids = [] + + with open(filepath, newline="") as f: + reader = csv.reader(f) + labels = [] + id_column = None + for rix, row in enumerate(reader): + if rix == 0: + labels = row + id_column = labels.index(header_label) + else: + # allow empty cells to keep consistency with row ordering in the sheet (for copy/paste) + ids.append(row[id_column]) + + return ids + + +# ----------------------------------------- +# Generates a report checking the success of blend to urdf parsing batches. +# e.g. python tools/generate_blend_to_urdf_parser_report.py --root-dir --report-model-list /all_scenes_artic_models-M1.csv +# e.g. add " --report-filepath 4, "Must provided more than the filetype." + report_filepath = args.report_filepath + + # scrape all existing subdirectories + exported_folder_names = find_subdirectory_names(root_dir=root_dir) + exported_folder_claimed = [False for exported_folder_name in exported_folder_names] + + # get model ids list + model_ids = exported_folder_names + if args.report_model_list is not None: + model_ids = load_model_list_from_csv(filepath=args.report_model_list) + + # for each model ids, check for existance of each expected output + for model_id in model_ids: + folder_path = os.path.join(root_dir, model_id) + folder_exists = False + if model_id in exported_folder_names: + folder_exists = True + # NOTE: silly override to + elif model_id + ".glb" in exported_folder_names: + folder_path = folder_path + ".glb" + folder_exists = True + + if folder_exists: + exported_folder_claimed[ + exported_folder_names.index(folder_path.split("/")[-1]) + ] = True + + urdf_exists = len(find_files(folder_path, file_endswith, ".urdf")) > 0 + + config_exists = ( + len(find_files(folder_path, file_endswith, ".ao_config.json")) > 0 + ) + + # NOTE: there could be missing assets here, but without parsing the blend file again, we wouldn't know. Heuristic is to expect at least one. + num_rec_meshes = len( + find_files(folder_path, file_endswith, "_receptacle.glb") + ) + one_receptacle_exists = num_rec_meshes > 0 + + one_render_mesh_exists = ( + len(find_files(folder_path, file_endswith, ".glb")) - num_rec_meshes + ) > 0 + + parse_results_report[model_id] = [ + model_id, + folder_exists, + urdf_exists, + config_exists, + one_receptacle_exists, + one_render_mesh_exists, + ] + else: + parse_results_report[model_id] = [False for i in range(len(cat_columns))] + parse_results_report[model_id][0] = model_id + + # export results to a file + os.makedirs(os.path.dirname(report_filepath), exist_ok=True) + with open(report_filepath, "w", newline="") as f: + writer = csv.writer(f, delimiter=",", quotechar="|", quoting=csv.QUOTE_MINIMAL) + # write the header labels + writer.writerow(cat_columns) + # write the contents + for model_id in model_ids: + writer.writerow(parse_results_report[model_id]) + + print("-----------------------------------------------") + print(f"Wrote report to {report_filepath}.\n") + + print("The following folders were unclaimed. Likely the root node is misnamed:") + for folder_index, claimed in enumerate(exported_folder_claimed): + if not claimed: + print(f" {exported_folder_names[folder_index]}") + print("-----------------------------------------------") + + +if __name__ == "__main__": + main() From d6a300e8dcf24982af5419976030c34638b99543 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 29 Nov 2023 14:33:59 -0800 Subject: [PATCH 105/153] force OBJECT context to avoid context errors with API --- tools/blender_armature_to_urdf.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index deefdea153..2fcf7b3a6a 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -634,6 +634,10 @@ def export( links = [] joints = [] + # check poll() to avoid exception. + if bpy.ops.object.mode_set.poll(): + bpy.ops.object.mode_set(mode="OBJECT") + # get the armature armature = settings.get("armature") if armature is None: From 49f916ed4237819e8f32df2a5fe1309045cb0cdb Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 4 Dec 2023 14:33:06 -0800 Subject: [PATCH 106/153] add option to export receptacles meshes as visual shapes in URDF --- tools/blender_armature_to_urdf.py | 43 ++++++++++++++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 2fcf7b3a6a..73412d388c 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -206,7 +206,11 @@ def add_color_material_to_visual(color, xml_visual) -> None: def bone_to_urdf( - this_bone, link_visuals=True, collision_visuals=False, joint_visuals=False + this_bone, + link_visuals=True, + collision_visuals=False, + joint_visuals=False, + receptacle_visuals=False, ): """This function extracts the basic properties of the bone and populates links and joint lists with the corresponding urdf nodes""" @@ -214,6 +218,7 @@ def bone_to_urdf( print(f"link_visuals = {link_visuals}") print(f"collision_visuals = {collision_visuals}") print(f"joint_visuals = {joint_visuals}") + print(f"receptacle_visuals = {receptacle_visuals}") global counter @@ -231,6 +236,7 @@ def bone_to_urdf( # TODO: scrape the list of mesh filenames which would be generated by an export. collision_objects = [] + receptacle_objects = [] for mesh_obj in bones_to_meshes[this_bone.name]: collision_objects.extend( get_mesh_heirarchy( @@ -240,6 +246,16 @@ def bone_to_urdf( include_render=False, ) ) + if receptacle_visuals: + receptacle_objects.extend( + get_mesh_heirarchy( + mesh_obj, + select_set=False, + include_collison=False, + include_render=False, + include_receptacle=True, + ) + ) if mesh_obj.parent is not None and mesh_obj.parent.type == "ARMATURE": # this object is the mesh name for export, so use it # Create the visual node @@ -343,6 +359,23 @@ def bone_to_urdf( this_xml_collision.append(this_xml_col_geom) this_xml_link.append(this_xml_collision) + if receptacle_visuals: + for rec_mesh in receptacle_objects: + # NOTE: color each link's collision shapes for debugging + this_color = get_next_color() + this_xml_visual = ET.Element("visual") + this_xml_geom = ET.Element("geometry") + this_xml_mesh = ET.Element("mesh") + rec_filename = rec_mesh.parent.name + "_receptacle.glb" + this_xml_mesh.set("filename", f"{rec_filename}") + this_xml_mesh.set("scale", "1.0 1.0 1.0") + this_xml_geom.append(this_xml_mesh) + # NOTE: we can use zero because we reset the origin for the meshes before exporting them to glb + this_xml_visual.append(ZERO_ORIGIN_NODE()) + this_xml_visual.append(this_xml_geom) + add_color_material_to_visual(this_color, this_xml_visual) + this_xml_link.append(this_xml_visual) + if not this_bone.children: pass # We reached the end of the chain. @@ -822,6 +855,7 @@ def export( link_visuals = True collision_visuals = False joint_visuals = False + receptacle_visuals = False # ----------------------------------------------------------- # ----------------------------------------------------------- @@ -878,6 +912,12 @@ def export( default=joint_visuals, help="Include visual box shapes for joint pivot locations in the exported URDF. E.g. for debugging.", ) + parser.add_argument( + "--receptacle-visuals", + action="store_true", + default=receptacle_visuals, + help="Include visual mesh shapes for receptacles in the exported URDF. E.g. for debugging.", + ) args = parser.parse_args(py_argv) @@ -903,5 +943,6 @@ def export( link_visuals=not args.no_link_visuals, collision_visuals=args.collision_visuals, joint_visuals=args.joint_visuals, + receptacle_visuals=args.receptacle_visuals, ) print(f"\n ======== Output saved to {output_path} ========\n") From 676a20193f4c777660264bd8ca0d9703ef10c2dd Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 4 Dec 2023 14:43:38 -0800 Subject: [PATCH 107/153] add option to round collision scales to 4 decimal places --- tools/blender_armature_to_urdf.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 73412d388c..c610b8b464 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -53,6 +53,17 @@ BASE_LIMIT_NODE_STR = None +def round_scales(keyword: str = "collision"): + """ + Rounds shape scale vectors to 4 decimal points (millimeter) accuracy. E.g. to eliminate small mismatches in collision shape scale. + Use 'keyword' to discriminate between shape types. + """ + for obj in bpy.context.scene.objects: + if keyword in obj.name: + for i in range(3): # Iterate over X, Y, Z + obj.scale[i] = round(obj.scale[i], 4) + + def set_base_limit_str(effort, velocity): """ Default effort and velocity limits for Joints. @@ -685,6 +696,9 @@ def export( if "joint_name_format" in settings: JOINT_NAME_FORMAT = settings["joint_name_format"] + if "round_collision_scales" in settings and settings["round_collision_scales"]: + round_scales() + # set the defaults to 100 T units and 3 units/sec (meters or radians) effort, velocity = (100, 3) if "def_limit_effort" in settings: @@ -850,6 +864,7 @@ def export( export_meshes = False export_ao_config = False + round_collision_scales = False # visual shape export flags for debugging link_visuals = True @@ -918,6 +933,12 @@ def export( default=receptacle_visuals, help="Include visual mesh shapes for receptacles in the exported URDF. E.g. for debugging.", ) + parser.add_argument( + "--round-collision-scales", + action="store_true", + default=round_collision_scales, + help="Round all scale elements for collision shapes to 4 decimal points (millimeter accuracy).", + ) args = parser.parse_args(py_argv) @@ -935,6 +956,7 @@ def export( export_path, { "armature": get_armature(), + "round_collision_scales": args.round_collision_scales #'def_limit_effort': 100, #custom default effort limit for joints #'def_limit_vel': 3, #custom default vel limit for joints }, From a70b18c0c9c7b6b4ec94cc605cc84bbb627f3875 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 4 Dec 2023 14:47:23 -0800 Subject: [PATCH 108/153] add option to flip negative scales for collision shapes --- tools/blender_armature_to_urdf.py | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index c610b8b464..892df95fef 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -64,6 +64,17 @@ def round_scales(keyword: str = "collision"): obj.scale[i] = round(obj.scale[i], 4) +def fix_scales(keyword: str = "collision"): + """ + Flips negative scales for shapes. (E.g. collision shapes should not have negative scaling) + Use 'keyword' to discriminate between shape types. + """ + for obj in bpy.context.scene.objects: + if keyword in obj.name: + for i in range(3): # Iterate over X, Y, Z + obj.scale[i] = abs(obj.scale[i]) + + def set_base_limit_str(effort, velocity): """ Default effort and velocity limits for Joints. @@ -699,6 +710,9 @@ def export( if "round_collision_scales" in settings and settings["round_collision_scales"]: round_scales() + if "fix_collision_scales" in settings and settings["fix_collision_scales"]: + fix_scales() + # set the defaults to 100 T units and 3 units/sec (meters or radians) effort, velocity = (100, 3) if "def_limit_effort" in settings: @@ -865,6 +879,7 @@ def export( export_meshes = False export_ao_config = False round_collision_scales = False + fix_collision_scales = False # visual shape export flags for debugging link_visuals = True @@ -939,6 +954,12 @@ def export( default=round_collision_scales, help="Round all scale elements for collision shapes to 4 decimal points (millimeter accuracy).", ) + parser.add_argument( + "--fix-collision-scales", + action="store_true", + default=fix_collision_scales, + help="Flip all negative scale elements for collision shapes.", + ) args = parser.parse_args(py_argv) @@ -956,7 +977,8 @@ def export( export_path, { "armature": get_armature(), - "round_collision_scales": args.round_collision_scales + "round_collision_scales": args.round_collision_scales, + "fix_collision_scales": args.fix_collision_scales, #'def_limit_effort': 100, #custom default effort limit for joints #'def_limit_vel': 3, #custom default vel limit for joints }, From 1f91c87cdb14eef4714380ff79ea3b0c01dfafd1 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 31 Jan 2024 11:32:51 -0800 Subject: [PATCH 109/153] minor update to output and valid urdf migration --- tools/batched_armature_to_urdf.py | 5 ++- tools/generate_blend_to_urdf_parser_report.py | 13 +++++++ ...place_articulated_models_in_rigid_scene.py | 39 ++++++++++++++++++- 3 files changed, 54 insertions(+), 3 deletions(-) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index 76ba9b5d71..1cb36e421b 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -74,7 +74,10 @@ def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: # first export the meshes os.system(base_command + " --export-meshes") # then export the URDF - os.system(base_command + " --export-ao-config") + os.system( + base_command + + " --export-ao-config --round-collision-scales --fix-collision-scales" + ) # ----------------------------------------- diff --git a/tools/generate_blend_to_urdf_parser_report.py b/tools/generate_blend_to_urdf_parser_report.py index b3bc17c505..16f9598895 100644 --- a/tools/generate_blend_to_urdf_parser_report.py +++ b/tools/generate_blend_to_urdf_parser_report.py @@ -125,6 +125,10 @@ def main(): "receptacles", "render_meshes", ] + # collect global sums of each error type + global_count: Dict[str, int] = {} + for check in cat_columns[1:]: + global_count[check] = 0 report_filepath = os.path.join(args.root_dir + "parser_report.csv") if args.report_filepath is not None: @@ -181,9 +185,16 @@ def main(): one_receptacle_exists, one_render_mesh_exists, ] + global_count["folder"] += int(not folder_exists) + global_count["config"] += int(not config_exists) + global_count["receptacles"] += int(not one_receptacle_exists) + global_count["urdf"] += int(not urdf_exists) + global_count["render_meshes"] += int(not one_render_mesh_exists) else: parse_results_report[model_id] = [False for i in range(len(cat_columns))] parse_results_report[model_id][0] = model_id + for key in global_count: + global_count[key] += 1 # export results to a file os.makedirs(os.path.dirname(report_filepath), exist_ok=True) @@ -204,6 +215,8 @@ def main(): print(f" {exported_folder_names[folder_index]}") print("-----------------------------------------------") + print(f"global_counts = {global_count}") + if __name__ == "__main__": main() diff --git a/tools/replace_articulated_models_in_rigid_scene.py b/tools/replace_articulated_models_in_rigid_scene.py index b1719af38e..7216f51fd8 100644 --- a/tools/replace_articulated_models_in_rigid_scene.py +++ b/tools/replace_articulated_models_in_rigid_scene.py @@ -120,7 +120,7 @@ def find_and_replace_articulated_models_for_config( def main(): parser = argparse.ArgumentParser( - description="Add user_defined entry to scene_instance.json files referencing the rec_filter_file." + description="Modify the scene_instance.json files, replacing rigid objects with articulated coutnerparts in a urdf/ directory." ) parser.add_argument( "--dataset-root-dir", @@ -140,8 +140,39 @@ def main(): configs = find_files(config_root_dir, file_is_scene_config) urdf_dir = os.path.join(fp_root_dir, "urdf/") urdf_files = find_files(urdf_dir, file_is_urdf) + + # create scene output directory + os.makedirs( + os.path.join(fp_root_dir, "scenes-articulated-uncluttered"), exist_ok=True + ) + + invalid_urdf_files = [] + + # only consider urdf files with reasonable accompanying contents + def urdf_has_meshes_and_config(urdf_filepath: str) -> bool: + """ + Return whether or not there are render meshes and a config accompanying the urdf. + """ + if not os.path.exists(urdf_filepath.split(".urdf")[0] + ".ao_config.json"): + return False + has_render_mesh = False + for file_name in os.listdir(os.path.dirname(urdf_filepath)): + if file_name.endswith(".glb") and "receptacle" not in file_name: + has_render_mesh = True + break + return has_render_mesh + + valid_urdf_files = [ + urdf_file for urdf_file in urdf_files if urdf_has_meshes_and_config(urdf_file) + ] + + invalid_urdf_files = [ + urdf_file for urdf_file in urdf_files if urdf_file not in valid_urdf_files + ] + urdf_names = [ - urdf_filename.split("/")[-1].split(".urdf")[0] for urdf_filename in urdf_files + urdf_filename.split("/")[-1].split(".urdf")[0] + for urdf_filename in valid_urdf_files ] # limit the scenes which are converted @@ -158,6 +189,10 @@ def main(): filepath, urdf_names=urdf_names, top_dir=fp_root_dir ) + print( + f"Migrated {len(valid_urdf_files)} urdfs into {len(configs)} scene configs. Invalid urdfs found and skipped ({len(invalid_urdf_files)}) = {invalid_urdf_files}" + ) + if __name__ == "__main__": main() From 5c8e09ea4a9b78d099bf93cfc7184e7ba61ac60c Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 9 Feb 2024 09:31:01 -0800 Subject: [PATCH 110/153] better error message --- tools/blender_armature_to_urdf.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 892df95fef..00ab3ee231 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -597,7 +597,13 @@ def get_anim_limits_info(bone): if len(limit_list) == 0: limit_list = [0, 0, 0] is_prismatic = True - limit_list[index] = value + try: + limit_list[index] = value + except IndexError: + raise Exception( + f"Failed to get limits for fcurve: bone={bone.name}, curve_key={_fkey}, index={index}. Should have exactly 3 (position) or exactly 4 (quaternion) elements." + ) + bone_limits[key_match] = limit_list assert ( is_prismatic or is_revolute From 4e34b1620506ec749f9eb9541975d963ecb93a1c Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 9 Feb 2024 13:44:15 -0800 Subject: [PATCH 111/153] try z up --- tools/blender_armature_to_urdf.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 00ab3ee231..5feeb6f56d 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -845,7 +845,7 @@ def export( "position": [0, 0, 0], "rotation": [1, 0, 0, 0], "scale": [1, 1, 1], - "up": [0, 1, 0], + "up": [0, 0, 1], "mesh_filepath": rec_name + ".glb", } ao_config_filename = os.path.join( @@ -858,7 +858,7 @@ def export( if __name__ == "__main__": - # NOTE: this must be run from within Blender and by defaults saves viles in "blender_armatures/" relative to the directory containing the script + # NOTE: this must be run from within Blender and by default saves files in "blender_armatures/" relative to the directory containing the script export_path = None try: From ab0ff01d5874fc5dc936b561dbebaf9a777f6a56 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 12 Feb 2024 12:22:39 -0800 Subject: [PATCH 112/153] support export of multi-mesh receptacles --- tools/blender_armature_to_urdf.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 5feeb6f56d..309548a6bd 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -746,9 +746,7 @@ def export( print(f" -pb> {obj.parent_bone}") if is_receptacle_mesh(obj): receptacle_meshes.append(obj) - receptacle_to_link_name[ - obj.parent.name + "_receptacle" - ] = obj.parent.name + receptacle_to_link_name[obj.name] = obj.parent.name elif obj.type == "EMPTY": print(f"EMPTY: {obj.name}") if obj.parent is None and len(obj.children) > 0: @@ -779,9 +777,7 @@ def export( deselect_all() rec_mesh.select_set(True) bpy.ops.export_scene.gltf( - filepath=os.path.join( - final_out_path, rec_mesh.parent.name + "_receptacle" - ), + filepath=os.path.join(final_out_path, rec_mesh.name), use_selection=True, export_yup=False, ) From 0d6b972608c1f5ac16b9ea82c19f7f45c842be47 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Thu, 14 Mar 2024 10:35:29 -0400 Subject: [PATCH 113/153] --remove erroneous ArticulatedObject's local initializationAttributes_ Should use the PhysicsObjectBase variable - this one was masking the desired variable. --- src/esp/physics/ArticulatedObject.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/esp/physics/ArticulatedObject.h b/src/esp/physics/ArticulatedObject.h index 90ddf74fa7..43d44362a6 100644 --- a/src/esp/physics/ArticulatedObject.h +++ b/src/esp/physics/ArticulatedObject.h @@ -896,12 +896,6 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { //! Cache the global scaling from the source model. Set during import. float globalScale_ = 1.0; - /** - * @brief Saved attributes when the object was initialized. - */ - metadata::attributes::ArticulatedObjectAttributes::ptr - initializationAttributes_ = nullptr; - public: ESP_SMART_POINTERS(ArticulatedObject) }; From 8987eca3075e6a4b13f78de6bdcea68a67c8a0e8 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Thu, 14 Mar 2024 11:33:39 -0400 Subject: [PATCH 114/153] --remove early exit; conditionally build urdf; fix ao config json save. --- tools/blender_armature_to_urdf.py | 76 ++++++++++++++++++------------- 1 file changed, 45 insertions(+), 31 deletions(-) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 309548a6bd..16ab5f902c 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -676,6 +676,7 @@ def construct_root_rotation_joint(root_node_name): def export( dirpath, settings, + export_urdf: bool = True, export_meshes: bool = True, export_ao_config: bool = True, **kwargs, @@ -757,6 +758,7 @@ def export( assert root_node is not None, "No root node, aborting." final_out_path = os.path.join(dirpath, f"{root_node.name}") os.makedirs(final_out_path, exist_ok=True) + print(f"Output path : {final_out_path}") # export mesh components if export_meshes: @@ -782,8 +784,6 @@ def export( export_yup=False, ) - return output_path - # print("------------------------") # print("Bone info recursion:") # walk_armature(root_bone, bone_info) @@ -791,38 +791,39 @@ def export( # print("Node info recursion:") # walk_armature(root_node, node_info) # print("------------------------") + if export_urdf: + # Recursively generate the xml elements + walk_armature(root_bone, bone_to_urdf, kwargs_for_handler=kwargs) - # Recursively generate the xml elements - walk_armature(root_bone, bone_to_urdf, kwargs_for_handler=kwargs) - - # add all the joints and links to the root - root_xml = ET.Element("robot") # create - root_xml.set("name", armature.name) + # add all the joints and links to the root + root_xml = ET.Element("robot") # create + root_xml.set("name", armature.name) - # add a coordinate change in a dummy root node - xml_root_link = ET.Element("link") - xml_root_link.set("name", "root") - xml_root_joint = construct_root_rotation_joint(root_bone.name) - root_xml.append(xml_root_link) - root_xml.append(xml_root_joint) + # add a coordinate change in a dummy root node + xml_root_link = ET.Element("link") + xml_root_link.set("name", "root") + xml_root_joint = construct_root_rotation_joint(root_bone.name) + root_xml.append(xml_root_link) + root_xml.append(xml_root_joint) - root_xml.append(ET.Comment("LINKS")) - for l in links: - root_xml.append(l) + root_xml.append(ET.Comment("LINKS")) + for l in links: + root_xml.append(l) - root_xml.append(ET.Comment("JOINTS")) - for j in joints: - root_xml.append(j) + root_xml.append(ET.Comment("JOINTS")) + for j in joints: + root_xml.append(j) - # dump the xml string - ET_raw_string = ET.tostring(root_xml, encoding="unicode") - dom = minidom.parseString(ET_raw_string) - ET_pretty_string = dom.toprettyxml() + # dump the xml string + ET_raw_string = ET.tostring(root_xml, encoding="unicode") + dom = minidom.parseString(ET_raw_string) + ET_pretty_string = dom.toprettyxml() - output_path = os.path.join(final_out_path, f"{root_node.name}.urdf") + output_path = os.path.join(final_out_path, f"{root_node.name}.urdf") - with open(output_path, "w") as f: - f.write(ET_pretty_string) + print(f"URDF output path : {output_path}") + with open(output_path, "w") as f: + f.write(ET_pretty_string) if export_ao_config: # write the ao_config @@ -847,8 +848,10 @@ def export( ao_config_filename = os.path.join( final_out_path, f"{root_node.name}.ao_config.json" ) + + print(f"ao config output path : {ao_config_filename}") with open(ao_config_filename, "w") as f: - f.write(json.dump(ao_config_contents, f)) + json.dump(ao_config_contents, f) return output_path @@ -878,6 +881,7 @@ def export( # Optionally override the save directory with an absolute path of your choice # export_path = "/home/my_path_choice/" + export_urdf = False export_meshes = False export_ao_config = False round_collision_scales = False @@ -892,7 +896,7 @@ def export( # ----------------------------------------------------------- # To use from the commandline: - # 1. `blender .blend --background --python blender_armature_to_urdf.py -- --export-path + # 1. `blender .blend --background --python blender_armature_to_urdf.py -- --export-path # 2. add `--export-meshes` to export the link .glbs # Note: ' -- ' tells Blender to ignore the remaining arguemnts, so we pass anything after that into the script arguements below: import sys @@ -924,7 +928,12 @@ def export( default=export_ao_config, help="Export a *.ao_config.json file for the URDF.", ) - + parser.add_argument( + "--export-urdf", + action="store_true", + default=export_urdf, + help="Export the *.urdf file.", + ) # Debugging flags: parser.add_argument( "--no-link-visuals", @@ -964,11 +973,15 @@ def export( ) args = parser.parse_args(py_argv) - + export_urdf = args.export_urdf export_meshes = args.export_meshes export_ao_config = args.export_ao_config export_path = args.export_path + print( + f"export_urdf : {export_urdf} | export_meshes : {export_meshes} | export_ao_config : {export_ao_config} | export_path : {export_path}" + ) + # ----------------------------------------------------------- assert ( @@ -984,6 +997,7 @@ def export( #'def_limit_effort': 100, #custom default effort limit for joints #'def_limit_vel': 3, #custom default vel limit for joints }, + export_urdf=export_urdf, export_meshes=export_meshes, export_ao_config=export_ao_config, link_visuals=not args.no_link_visuals, From 045956944f703b5e9c5e2138a5a887c22babe346 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Tue, 19 Mar 2024 10:11:27 -0400 Subject: [PATCH 115/153] --add ability to just see kitchen regions Also fix remaking region colors when tabbing to new scene. NOTE : Receptacle setup still does not support scene tabbing. --- examples/viewer.py | 67 +++++++++++++++++++++++++++++----------------- 1 file changed, 43 insertions(+), 24 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index ba7a90d311..bc9f5031df 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -134,16 +134,6 @@ def __init__( self.sim_settings["width"] = camera_resolution[0] self.sim_settings["height"] = camera_resolution[1] - # draw Bullet debug line visualizations (e.g. collision meshes) - self.debug_bullet_draw = False - # draw active contact point debug line visualizations - self.contact_debug_draw = False - # draw semantic region debug visualizations if present - self.semantic_region_debug_draw = False - - # cache most recently loaded URDF file for quick-reload - self.cached_urdf = "" - # set up our movement map key = Application.KeyEvent.Key self.pressed = { @@ -221,6 +211,18 @@ def __init__( # variables that track app data and CPU/GPU usage self.num_frames_to_track = 60 + # Descriptive strings for semantic region debug draw possible choices + self.semantic_region_debug_draw_choices = ["None", "Kitchen Only", "All"] + + # draw Bullet debug line visualizations (e.g. collision meshes) + self.debug_bullet_draw = False + # draw active contact point debug line visualizations + self.contact_debug_draw = False + # draw semantic region debug visualizations if present : should be [0 : len(semantic_region_debug_draw_choices)-1] + self.semantic_region_debug_draw_state = 0 + + # cache most recently loaded URDF file for quick-reload + self.cached_urdf = "" # Cycle mouse utilities self.mouse_interaction = MouseMode.LOOK @@ -592,15 +594,27 @@ def draw_region_debug(self, debug_line_render: Any) -> None: """ Draw the semantic region wireframes. """ - - for region in self.sim.semantic_scene.regions: - color = self.debug_semantic_colors.get(region.id, mn.Color4.magenta()) - for edge in region.volume_edges: - debug_line_render.draw_transformed_line( - edge[0], - edge[1], - color, - ) + if self.semantic_region_debug_draw_state == 1: + for region in self.sim.semantic_scene.regions: + if "kitchen" not in region.id.lower(): + continue + color = self.debug_semantic_colors.get(region.id, mn.Color4.magenta()) + for edge in region.volume_edges: + debug_line_render.draw_transformed_line( + edge[0], + edge[1], + color, + ) + else: + # Draw all + for region in self.sim.semantic_scene.regions: + color = self.debug_semantic_colors.get(region.id, mn.Color4.magenta()) + for edge in region.volume_edges: + debug_line_render.draw_transformed_line( + edge[0], + edge[1], + color, + ) def debug_draw(self): """ @@ -615,8 +629,10 @@ def debug_draw(self): debug_line_render = self.sim.get_debug_line_render() if self.contact_debug_draw: self.draw_contact_debug(debug_line_render) - if self.semantic_region_debug_draw: + + if self.semantic_region_debug_draw_state != 0: if len(self.debug_semantic_colors) != len(self.sim.semantic_scene.regions): + self.debug_semantic_colors = {} for region in self.sim.semantic_scene.regions: self.debug_semantic_colors[region.id] = mn.Color4( mn.Vector3(np.random.random(3)) @@ -1040,11 +1056,14 @@ def key_press_event(self, event: Application.KeyEvent) -> None: elif key == pressed.H: self.print_help_text() elif key == pressed.J: + new_state_idx = (self.semantic_region_debug_draw_state + 1) % len( + self.semantic_region_debug_draw_choices + ) logger.info( - f"Toggle Region Draw from {self.semantic_region_debug_draw } to {not self.semantic_region_debug_draw}" + f"Change Region Draw from {self.semantic_region_debug_draw_choices[self.semantic_region_debug_draw_state]} to {self.semantic_region_debug_draw_choices[new_state_idx]}" ) - # Toggle visualize semantic bboxes. Currently only regions supported - self.semantic_region_debug_draw = not self.semantic_region_debug_draw + # Increment visualize semantic bboxes. Currently only regions supported + self.semantic_region_debug_draw_state = new_state_idx elif key == pressed.TAB: # NOTE: (+ALT) - reconfigure without cycling scenes @@ -1904,7 +1923,7 @@ def next_frame() -> None: def init_cpo_for_scene(sim_settings, mm: habitat_sim.metadata.MetadataMediator): """ - Initialize and run th CPO for all objects in the scene. + Initialize and run the CPO for all objects in the scene. """ global _cpo global _cpo_threads From 5dc4cc65a1a33f13b24a6d3eafa2424f6d05205b Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Tue, 19 Mar 2024 10:27:58 -0400 Subject: [PATCH 116/153] --reorganize ctor code to collect per-scene init code This is to eventually support scene tabbing. Still need to modfiy the CSA/CPO shape generation to be per scene (and to not rebuild sim multiple times). --- examples/viewer.py | 38 +++++++++++++++++++++++--------------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index bc9f5031df..c8ecf1e207 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -214,12 +214,28 @@ def __init__( # Descriptive strings for semantic region debug draw possible choices self.semantic_region_debug_draw_choices = ["None", "Kitchen Only", "All"] + global _cpo + self._cpo = _cpo + self.cpo_initialized = False + self.proxy_obj_postfix = "_collision_stand-in" + + # initialization code below here + # TODO isolate all initialization so tabbing through scenes can be properly supported + # configure our simulator + self.cfg: Optional[habitat_sim.simulator.Configuration] = None + self.sim: Optional[habitat_sim.simulator.Simulator] = None + self.tiled_sims: list[habitat_sim.simulator.Simulator] = None + self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None + self.replay_renderer: Optional[ReplayRenderer] = None + # draw Bullet debug line visualizations (e.g. collision meshes) self.debug_bullet_draw = False # draw active contact point debug line visualizations self.contact_debug_draw = False # draw semantic region debug visualizations if present : should be [0 : len(semantic_region_debug_draw_choices)-1] self.semantic_region_debug_draw_state = 0 + # Colors to use for each region's semantic rendering. + self.debug_semantic_colors = {} # cache most recently loaded URDF file for quick-reload self.cached_urdf = "" @@ -231,13 +247,13 @@ def __init__( # toggle physics simulation on/off self.simulating = False + # toggle a single simulation step at the next opportunity if not + # simulating continuously. + self.simulate_single_step = False # receptacle visualization self.receptacles = None self.display_receptacles = False - global _cpo - self._cpo = _cpo - self.cpo_initialized = False self.show_filtered = True self.rec_access_filter_threshold = 0.12 # empirically chosen self.rec_color_mode = RecColorMode.FILTERING @@ -245,6 +261,8 @@ def __init__( self.rec_to_poh: Dict[hab_receptacle.Receptacle, str] = {} # contains filtering metadata and classification of meshes filtered automatically and manually self.rec_filter_data = None + # TODO need to determine filter path for each scene during tabbing? + # Currently this field is only set as command-line argument self.rec_filter_path = self.sim_settings["rec_filter_file"] # display stability samples for selected object w/ receptacle @@ -254,7 +272,6 @@ def __init__( self.col_proxy_objs = None self.col_proxies_visible = True self.original_objs_visible = True - self.proxy_obj_postfix = "_collision_stand-in" # mouse raycast visualization self.mouse_cast_results = None @@ -263,18 +280,9 @@ def __init__( self.selected_rec = None self.ao_link_map = None - # toggle a single simulation step at the next opportunity if not - # simulating continuously. - self.simulate_single_step = False - - # configure our simulator - self.cfg: Optional[habitat_sim.simulator.Configuration] = None - self.sim: Optional[habitat_sim.simulator.Simulator] = None - self.tiled_sims: list[habitat_sim.simulator.Simulator] = None - self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None - self.replay_renderer: Optional[ReplayRenderer] = None + # Sim reconfigure self.reconfigure_sim(mm) - self.debug_semantic_colors = {} + # load appropriate filter file for scene self.load_scene_filter_file() # ----------------------------------------- From 74c65222aab60876133e6fba7c8f6cf4c5a9d059 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 20 Mar 2024 11:43:44 -0700 Subject: [PATCH 117/153] clear joint states on spot_viewer save with 'i' --- examples/spot_viewer.py | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index b3fac577b7..628dfbc893 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -666,6 +666,17 @@ def reconfigure_sim(self) -> None: for composite_file in sim_settings["composite_files"]: self.replay_renderer.preload_file(composite_file) + # check that clearing joint positions on save won't corrupt the content + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + for joint_val in ao.joint_positions: + assert ( + joint_val == 0 + ), "If this fails, there are non-zero joint positions in the scene_instance or default pose. Export with 'i' will clear these." + # add the robot to the world via the wrapper robot_path = "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" agent_config = DictConfig({"articulated_agent_urdf": robot_path}) @@ -892,7 +903,16 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # f.write(json.dumps(self.modified_objects_buffer, indent=2)) aom = self.sim.get_articulated_object_manager() aom.remove_object_by_handle(self.spot.sim_obj.handle) - # aom.remove_all_objects() + + # clear furniture joint positions before saving + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + j_pos = ao.joint_positions + ao.joint_positions = [0.0 for _ in range(len(j_pos))] + self.sim.save_current_scene_config(overwrite=True) print("Saved modified scene instance JSON to original location.") # de-duplicate and save clutter list From a9d62da916b7bed42afbef2956d3f125960ed378 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 20 Mar 2024 11:48:45 -0700 Subject: [PATCH 118/153] add a keybind 'j' for clearing joint states manually --- examples/spot_viewer.py | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 628dfbc893..207715cf4b 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -383,6 +383,20 @@ def place_spot(self): if valid_spot_point is not None: self.spot.base_pos = valid_spot_point + def clear_furniture_joint_states(self): + """ + Clear all furniture object joint states. + """ + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + # ignore the robot + if "hab_spot" not in ao.handle: + j_pos = ao.joint_positions + ao.joint_positions = [0.0 for _ in range(len(j_pos))] + def draw_contact_debug(self): """ This method is called to render a debug line overlay displaying active contact points and normals. @@ -896,6 +910,9 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.selected_object = None self.navmesh_config_and_recompute() + elif key == pressed.J: + self.clear_furniture_joint_states() + elif key == pressed.I: # dump the modified object states buffer to JSON. # print(f"Writing modified_objects_buffer to 'scene_mod_buffer.json': {self.modified_objects_buffer}") @@ -905,13 +922,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: aom.remove_object_by_handle(self.spot.sim_obj.handle) # clear furniture joint positions before saving - for ao in ( - self.sim.get_articulated_object_manager() - .get_objects_by_handle_substring() - .values() - ): - j_pos = ao.joint_positions - ao.joint_positions = [0.0 for _ in range(len(j_pos))] + self.clear_furniture_joint_states() self.sim.save_current_scene_config(overwrite=True) print("Saved modified scene instance JSON to original location.") From 532984019b598ae36304b26b04ab17df7daec0a0 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 20 Mar 2024 11:54:34 -0700 Subject: [PATCH 119/153] include AOs on the navmesh for spot_viewer --- examples/spot_viewer.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 207715cf4b..7cd59b7d10 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -1081,8 +1081,32 @@ def navmesh_config_and_recompute(self) -> None: self.navmesh_settings.agent_height = self.cfg.agents[self.agent_id].height self.navmesh_settings.agent_radius = 0.3 self.navmesh_settings.include_static_objects = True + + # first cache AO motion types and set to STATIC for navmesh + ao_motion_types = {} + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + # ignore the robot + if "hab_spot" not in ao.handle: + ao_motion_types[ao.handle] = ao.motion_type + ao.motion_type = habitat_sim.physics.MotionType.STATIC + self.sim.recompute_navmesh(self.sim.pathfinder, self.navmesh_settings) + # reset AO motion types from cache + ao_motion_types = {} + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + # ignore the robot + if ao.handle in ao_motion_types: + ao.motion_type = ao_motion_types[ao.handle] + def exit_event(self, event: Application.ExitEvent): """ Overrides exit_event to properly close the Simulator before exiting the From f39b0597834c6d9274628bce127bbd6c8474d5b3 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Wed, 20 Mar 2024 14:58:18 -0700 Subject: [PATCH 120/153] add script to automatically check siro scenes with initial joint popping check --- tools/check_siro_scenes.py | 174 +++++++++++++++++++++++++++++++++++++ 1 file changed, 174 insertions(+) create mode 100644 tools/check_siro_scenes.py diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py new file mode 100644 index 0000000000..06f98426f5 --- /dev/null +++ b/tools/check_siro_scenes.py @@ -0,0 +1,174 @@ +import os +from typing import Any, Dict, List + +from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer + +from habitat_sim import Simulator +from habitat_sim.metadata import MetadataMediator +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +# TODO: get metadata for semantics +# from habitat_llm.dataset_generation.templated.generate_episodes import MetadataInterface + + +def get_labels_from_dict(results_dict: Dict[str, Dict[str, Any]]) -> List[str]: + """ + Get a list of column labels for the csv by scraping dict keys from the inner dict layers. + """ + labels = [] + for scene_dict in results_dict.values(): + for dict_key in scene_dict: + if dict_key not in labels: + labels.append(dict_key) + return labels + + +def export_results_csv(filepath: str, results_dict: Dict[str, Dict[str, Any]]) -> None: + assert filepath.endswith(".csv") + + col_labels = get_labels_from_dict(results_dict) + + with open(filepath, "w") as f: + # first write the column labels + f.write("scene,") + for c_label in col_labels: + f.write(f"{c_label},") + f.write("\n") + + # now a row for each scene + for scene_handle, scene_dict in results_dict.items(): + # write the scene column + f.write(f"{scene_handle},") + for label in col_labels: + if label in scene_dict: + f.write(f"{scene_dict[label]},") + else: + f.write(",") + f.write("\n") + print(f"Wrote results csv to {filepath}") + + +def check_joint_popping( + sim: Simulator, out_dir: str = None, dbv: DebugVisualizer = None +) -> List[str]: + """ + Get a list of ao handles for objects which are not stable during simulation. + Checks the initial joint state, then simulates 1 second, then check the joint state again. Changes indicate popping, collisions, loose hinges, or other instability. + + :param out_dir: If provided, save debug images to the output directory prefixed "joint_pop____". + """ + + if out_dir is not None and dbv is None: + dbv = DebugVisualizer(sim) + + # record the ao handles + unstable_aos = [] + # record the sum of errors across all joints + cumulative_errors = [] + + ao_initial_joint_states = {} + + for ao_handle, ao in ( + sim.get_articulated_object_manager().get_objects_by_handle_substring().items() + ): + ao_initial_joint_states[ao_handle] = ao.joint_positions + + sim.step_physics(2.0) + + # cumulative error must be above this threshold to count as "unstable" + eps = 1e-3 + + for ao_handle, ao in ( + sim.get_articulated_object_manager().get_objects_by_handle_substring().items() + ): + jp = ao.joint_positions + if ao_initial_joint_states[ao_handle] != jp: + cumulative_error = sum( + [ + abs(ao_initial_joint_states[ao_handle][i] - jp[i]) + for i in range(len(jp)) + ] + ) + if cumulative_error > eps: + cumulative_errors.append(cumulative_error) + unstable_aos.append(ao_handle) + if out_dir is not None: + dbv.peek(ao_handle, peek_all_axis=True).save( + output_path=out_dir, prefix=f"joint_pop__{ao_handle}__" + ) + + return unstable_aos, cumulative_errors + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + + parser.add_argument( + "--dataset", + default="default", + type=str, + metavar="DATASET", + help='dataset configuration file to use (default: "default")', + ) + parser.add_argument( + "--out-dir", + default="siro_test_results/", + type=str, + help="directory in which to cache images and results csv.", + ) + parser.add_argument( + "--save-images", + default=False, + action="store_true", + help="save images during tests into the output directory.", + ) + args = parser.parse_args() + + os.makedirs(args.out_dir, exist_ok=True) + + # create an initial simulator config + sim_settings: Dict[str, Any] = default_sim_settings + sim_settings["scene_dataset_config_file"] = args.dataset + cfg = make_cfg(sim_settings) + + # pre-initialize a MetadataMediator to iterate over scenes + mm = MetadataMediator() + mm.active_dataset = args.dataset + cfg.metadata_mediator = mm + + # keyed by scene handle + scene_test_results: Dict[str, Dict[str, Any]] = {} + + # for each scene, initialize a fresh simulator and run tests + for scene_handle in mm.get_scene_handles(): + print(f"Setting up scene for {scene_handle}") + cfg.sim_cfg.scene_id = scene_handle + with Simulator(cfg) as sim: + dbv = DebugVisualizer(sim) + + scene_test_results[sim.curr_scene_name] = {} + scene_test_results[sim.curr_scene_name][ + "ros" + ] = sim.get_rigid_object_manager().get_num_objects() + scene_test_results[sim.curr_scene_name][ + "aos" + ] = sim.get_articulated_object_manager().get_num_objects() + + scene_out_dir = os.path.join(args.out_dir, f"{sim.curr_scene_name}/") + + ########################################## + # Check for joint popping + unstable_aos, joint_errors = check_joint_popping( + sim, out_dir=scene_out_dir if args.save_images else None, dbv=dbv + ) + if len(unstable_aos) > 0: + scene_test_results[sim.curr_scene_name]["unstable_aos"] = "" + for ix, ao_handle in enumerate(unstable_aos): + scene_test_results[sim.curr_scene_name][ + "unstable_aos" + ] += f"{ao_handle}({joint_errors[ix]}) | " + + csv_filepath = os.path.join(args.out_dir, "siro_scene_test_results.csv") + export_results_csv(csv_filepath, scene_test_results) From 63f3661e179854f8a851e6712960a8c81e9d593e Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Thu, 21 Mar 2024 11:17:11 -0400 Subject: [PATCH 121/153] --update spot viewer have new fix for shadowy text; use appropriate flags --- examples/spot_viewer.py | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 7cd59b7d10..3a792c8826 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -630,8 +630,8 @@ def reconfigure_sim(self) -> None: self.cfg.sim_cfg.create_renderer = False self.cfg.sim_cfg.enable_gfx_replay_save = True - if self.sim_settings["stage_requires_lighting"]: - logger.info("Setting synthetic lighting override for stage.") + if self.sim_settings["use_default_lighting"]: + logger.info("Setting default lighting override for scene.") self.cfg.sim_cfg.override_scene_light_defaults = True self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY @@ -729,7 +729,7 @@ def move_and_look(self, repetitions: int) -> None: return key = Application.KeyEvent.Key - press: Dict[key.key, bool] = self.pressed + press: Dict[Application.KeyEvent.Key.key, bool] = self.pressed inc = 0.02 min_val = 0.1 @@ -1118,6 +1118,13 @@ def exit_event(self, event: Application.ExitEvent): exit(0) def draw_text(self, sensor_spec): + # make magnum text background transparent for text + mn.gl.Renderer.enable(mn.gl.Renderer.Feature.BLENDING) + mn.gl.Renderer.set_blend_function( + mn.gl.Renderer.BlendFunction.ONE, + mn.gl.Renderer.BlendFunction.ONE_MINUS_SOURCE_ALPHA, + ) + self.shader.bind_vector_texture(self.glyph_cache.texture) self.shader.transformation_projection_matrix = self.window_text_transform self.shader.color = [1.0, 1.0, 1.0] @@ -1127,12 +1134,16 @@ def draw_text(self, sensor_spec): self.window_text.render( f""" {self.fps} FPS +Scene ID : {os.path.split(self.cfg.sim_cfg.scene_id)[1].split('.scene_instance')[0]} Sensor Type: {sensor_type_string} Sensor Subtype: {sensor_subtype_string} """ ) self.shader.draw(self.window_text.mesh) + # Disable blending for text + mn.gl.Renderer.disable(mn.gl.Renderer.Feature.BLENDING) + def print_help_text(self) -> None: """ Print the Key Command help text. @@ -1254,9 +1265,14 @@ def next_frame() -> None: help="disable physics simulation (default: False)", ) parser.add_argument( - "--stage-requires-lighting", + "--use-default-lighting", + action="store_true", + help="Override configured lighting to use default lighting for the stage.", + ) + parser.add_argument( + "--hbao", action="store_true", - help="Override configured lighting to use synthetic lighting for the stage.", + help="Enable horizon-based ambient occlusion, which provides soft shadows in corners and crevices.", ) parser.add_argument( "--enable-batch-renderer", @@ -1302,14 +1318,14 @@ def next_frame() -> None: sim_settings["scene"] = args.scene sim_settings["scene_dataset_config_file"] = args.dataset sim_settings["enable_physics"] = not args.disable_physics - sim_settings["stage_requires_lighting"] = args.stage_requires_lighting + sim_settings["use_default_lighting"] = args.use_default_lighting sim_settings["enable_batch_renderer"] = args.enable_batch_renderer sim_settings["num_environments"] = args.num_environments sim_settings["composite_files"] = args.composite_files sim_settings["window_width"] = args.width sim_settings["window_height"] = args.height sim_settings["sensor_height"] = 0 - sim_settings["enable_hbao"] = True + sim_settings["enable_hbao"] = args.hbao # start the application HabitatSimInteractiveViewer(sim_settings).exec() From 131fc46ddb86d7a197ee9b1c86c4c22925c8ec5e Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Thu, 21 Mar 2024 11:27:09 -0400 Subject: [PATCH 122/153] --allow casting/obj selecttion through scene ceiling/roof Sets first non-stage object as selected unless there is none; treats as stage hit otherwise --- examples/spot_viewer.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 3a792c8826..391dfefe16 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -1013,15 +1013,22 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: ray = render_camera.unproject(self.get_mouse_position(event.position)) mouse_cast_results = self.sim.cast_ray(ray=ray) if mouse_cast_results.has_hits(): - self.last_hit_details = mouse_cast_results.hits[0] - hit_id = mouse_cast_results.hits[0].object_id - self.selected_object = sutils.get_obj_from_id(self.sim, hit_id) - if self.selected_object is None: - print("This is the stage.") - else: + hit_idx = 0 + obj_found = False + while hit_idx < len(mouse_cast_results.hits) and not obj_found: + self.last_hit_details = mouse_cast_results.hits[hit_idx] + hit_obj_id = mouse_cast_results.hits[hit_idx].object_id + self.selected_object = sutils.get_obj_from_id(self.sim, hit_obj_id) + if self.selected_object is None: + hit_idx += 1 + else: + obj_found = True + if obj_found: print( f"Object: {self.selected_object.handle} is {type(self.selected_object)}" ) + else: + print("This is the stage.") self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() From 8917f496c5461c3a6d3c7e0ad1d6d212a2b1b222 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Thu, 21 Mar 2024 11:29:02 -0400 Subject: [PATCH 123/153] --clean up viewer some --- examples/viewer.py | 314 +++++++++++++++++++++++---------------------- 1 file changed, 160 insertions(+), 154 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index c8ecf1e207..d6f2f7bea1 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -628,7 +628,6 @@ def debug_draw(self): """ Additional draw commands to be called during draw_event. """ - self.sim.get_rigid_object_manager() if self.debug_bullet_draw: render_cam = self.render_camera.render_camera proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) @@ -1042,6 +1041,32 @@ def invert_gravity(self) -> None: gravity: mn.Vector3 = self.sim.get_gravity() * -1 self.sim.set_gravity(gravity) + def cycleScene(self, change_scene: bool, shift_pressed: bool): + if change_scene: + # cycle the active scene from the set available in MetadataMediator + inc = -1 if shift_pressed else 1 + scene_ids = self.sim.metadata_mediator.get_scene_handles() + cur_scene_index = 0 + if self.sim_settings["scene"] not in scene_ids: + matching_scenes = [ + (ix, x) + for ix, x in enumerate(scene_ids) + if self.sim_settings["scene"] in x + ] + if not matching_scenes: + logger.warning( + f"The current scene, '{self.sim_settings['scene']}', is not in the list, starting cycle at index 0." + ) + else: + cur_scene_index = matching_scenes[0][0] + else: + cur_scene_index = scene_ids.index(self.sim_settings["scene"]) + + next_scene_index = min(max(cur_scene_index + inc, 0), len(scene_ids) - 1) + self.sim_settings["scene"] = scene_ids[next_scene_index] + self.reconfigure_sim() + logger.info(f"Reconfigured simulator for scene: {self.sim_settings['scene']}") + def key_press_event(self, event: Application.KeyEvent) -> None: """ Handles `Application.KeyEvent` on a key press by performing the corresponding functions. @@ -1061,48 +1086,13 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.exit_event(Application.ExitEvent) return - elif key == pressed.H: - self.print_help_text() - elif key == pressed.J: - new_state_idx = (self.semantic_region_debug_draw_state + 1) % len( - self.semantic_region_debug_draw_choices - ) - logger.info( - f"Change Region Draw from {self.semantic_region_debug_draw_choices[self.semantic_region_debug_draw_state]} to {self.semantic_region_debug_draw_choices[new_state_idx]}" - ) - # Increment visualize semantic bboxes. Currently only regions supported - self.semantic_region_debug_draw_state = new_state_idx + elif key == pressed.SIX: + # Reset mouse wheel FOV zoom + self.render_camera.reset_zoom() elif key == pressed.TAB: - # NOTE: (+ALT) - reconfigure without cycling scenes - if not alt_pressed: - # cycle the active scene from the set available in MetadataMediator - inc = -1 if shift_pressed else 1 - scene_ids = self.sim.metadata_mediator.get_scene_handles() - cur_scene_index = 0 - if self.sim_settings["scene"] not in scene_ids: - matching_scenes = [ - (ix, x) - for ix, x in enumerate(scene_ids) - if self.sim_settings["scene"] in x - ] - if not matching_scenes: - logger.warning( - f"The current scene, '{self.sim_settings['scene']}', is not in the list, starting cycle at index 0." - ) - else: - cur_scene_index = matching_scenes[0][0] - else: - cur_scene_index = scene_ids.index(self.sim_settings["scene"]) - - next_scene_index = min( - max(cur_scene_index + inc, 0), len(scene_ids) - 1 - ) - self.sim_settings["scene"] = scene_ids[next_scene_index] - self.reconfigure_sim() - logger.info( - f"Reconfigured simulator for scene: {self.sim_settings['scene']}" - ) + # Cycle through scenes + self.cycleScene(True, shift_pressed=shift_pressed) elif key == pressed.SPACE: if not self.sim.config.sim_cfg.enable_physics: @@ -1137,6 +1127,123 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.contact_debug_draw = True # TODO: add a nice log message with concise contact pair naming. + elif key == pressed.F: + # toggle, load(+ALT), or save(+SHIFT) filtering + if shift_pressed and self.rec_filter_data is not None: + self.export_filtered_recs(self.rec_filter_path) + elif alt_pressed: + self.load_filtered_recs(self.rec_filter_path) + else: + self.show_filtered = not self.show_filtered + print(f"self.show_filtered = {self.show_filtered}") + + elif key == pressed.H: + self.print_help_text() + + elif key == pressed.J: + new_state_idx = (self.semantic_region_debug_draw_state + 1) % len( + self.semantic_region_debug_draw_choices + ) + logger.info( + f"Change Region Draw from {self.semantic_region_debug_draw_choices[self.semantic_region_debug_draw_state]} to {self.semantic_region_debug_draw_choices[new_state_idx]}" + ) + # Increment visualize semantic bboxes. Currently only regions supported + self.semantic_region_debug_draw_state = new_state_idx + + elif key == pressed.M: + self.cycle_mouse_mode() + logger.info(f"Command: mouse mode set to {self.mouse_interaction}") + + elif key == pressed.N: + # (default) - toggle navmesh visualization + # NOTE: (+ALT) - re-sample the agent position on the NavMesh + # NOTE: (+SHIFT) - re-compute the NavMesh + if alt_pressed: + logger.info("Command: resample agent state from navmesh") + if self.sim.pathfinder.is_loaded: + new_agent_state = habitat_sim.AgentState() + largest_island_ix = get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) + print(f"Largest indoor island index = {largest_island_ix}") + new_agent_state.position = ( + self.sim.pathfinder.get_random_navigable_point( + island_index=largest_island_ix + ) + ) + new_agent_state.rotation = quat_from_angle_axis( + self.sim.random.uniform_float(0, 2.0 * np.pi), + np.array([0, 1, 0]), + ) + self.default_agent.set_state(new_agent_state) + else: + logger.warning( + "NavMesh is not initialized. Cannot sample new agent state." + ) + elif shift_pressed: + logger.info("Command: recompute navmesh") + self.navmesh_config_and_recompute() + else: + if self.sim.pathfinder.is_loaded: + self.sim.navmesh_visualization = not self.sim.navmesh_visualization + logger.info("Command: toggle navmesh") + else: + logger.warn("Warning: recompute navmesh first") + + elif key == pressed.O: + if shift_pressed: + # move non-proxy objects in/out of visible space + self.original_objs_visible = not self.original_objs_visible + print(f"self.original_objs_visible = {self.original_objs_visible}") + if not self.original_objs_visible: + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + obj.translation = obj.translation + mn.Vector3(200, 0, 0) + obj.motion_type = habitat_sim.physics.MotionType.STATIC + else: + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + obj.translation = obj.translation - mn.Vector3(200, 0, 0) + obj.motion_type = habitat_sim.physics.MotionType.STATIC + else: + if self.col_proxy_objs is None: + self.col_proxy_objs = [] + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + # add a new proxy object + self.col_proxy_objs.append(self.add_col_proxy_object(obj)) + else: + self.col_proxies_visible = not self.col_proxies_visible + print(f"self.col_proxies_visible = {self.col_proxies_visible}") + + # make the proxies visible or not by moving them + if not self.col_proxies_visible: + for obj in self.col_proxy_objs: + obj.translation = obj.translation + mn.Vector3(200, 0, 0) + else: + for obj in self.col_proxy_objs: + obj.translation = obj.translation - mn.Vector3(200, 0, 0) + + elif key == pressed.R: + # Reload current scene + self.cycleScene(False, shift_pressed=shift_pressed) + elif key == pressed.T: self.modify_param_from_term() @@ -1183,35 +1290,6 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # else: # logger.warn("Load URDF: input file not found. Aborting.") - elif key == pressed.M: - self.cycle_mouse_mode() - logger.info(f"Command: mouse mode set to {self.mouse_interaction}") - - elif key == pressed.V: - # load receptacles and toggle visibilty or color mode (+SHIFT) - if self.receptacles is None: - self.load_receptacles() - - if shift_pressed: - self.rec_color_mode = RecColorMode( - (self.rec_color_mode.value + 1) % len(RecColorMode) - ) - print(f"self.rec_color_mode = {self.rec_color_mode}") - self.display_receptacles = True - else: - self.display_receptacles = not self.display_receptacles - print(f"self.display_receptacles = {self.display_receptacles}") - - elif key == pressed.F: - # toggle, load(+ALT), or save(+SHIFT) filtering - if shift_pressed and self.rec_filter_data is not None: - self.export_filtered_recs(self.rec_filter_path) - elif alt_pressed: - self.load_filtered_recs(self.rec_filter_path) - else: - self.show_filtered = not self.show_filtered - print(f"self.show_filtered = {self.show_filtered}") - elif key == pressed.U: rom = self.sim.get_rigid_object_manager() # add objects to the selected receptacle or remove al objects @@ -1278,92 +1356,20 @@ def key_press_event(self, event: Application.KeyEvent) -> None: else: print("No object selected, cannot sample clutter.") - elif key == pressed.O: - if shift_pressed: - # move non-proxy objects in/out of visible space - self.original_objs_visible = not self.original_objs_visible - print(f"self.original_objs_visible = {self.original_objs_visible}") - if not self.original_objs_visible: - for _obj_handle, obj in ( - self.sim.get_rigid_object_manager() - .get_objects_by_handle_substring() - .items() - ): - if self.proxy_obj_postfix not in obj.creation_attributes.handle: - obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC - obj.translation = obj.translation + mn.Vector3(200, 0, 0) - obj.motion_type = habitat_sim.physics.MotionType.STATIC - else: - for _obj_handle, obj in ( - self.sim.get_rigid_object_manager() - .get_objects_by_handle_substring() - .items() - ): - if self.proxy_obj_postfix not in obj.creation_attributes.handle: - obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC - obj.translation = obj.translation - mn.Vector3(200, 0, 0) - obj.motion_type = habitat_sim.physics.MotionType.STATIC - - else: - if self.col_proxy_objs is None: - self.col_proxy_objs = [] - for _obj_handle, obj in ( - self.sim.get_rigid_object_manager() - .get_objects_by_handle_substring() - .items() - ): - if self.proxy_obj_postfix not in obj.creation_attributes.handle: - # add a new proxy object - self.col_proxy_objs.append(self.add_col_proxy_object(obj)) - else: - self.col_proxies_visible = not self.col_proxies_visible - print(f"self.col_proxies_visible = {self.col_proxies_visible}") - - # make the proxies visible or not by moving them - if not self.col_proxies_visible: - for obj in self.col_proxy_objs: - obj.translation = obj.translation + mn.Vector3(200, 0, 0) - else: - for obj in self.col_proxy_objs: - obj.translation = obj.translation - mn.Vector3(200, 0, 0) + elif key == pressed.V: + # load receptacles and toggle visibilty or color mode (+SHIFT) + if self.receptacles is None: + self.load_receptacles() - elif key == pressed.N: - # (default) - toggle navmesh visualization - # NOTE: (+ALT) - re-sample the agent position on the NavMesh - # NOTE: (+SHIFT) - re-compute the NavMesh - if alt_pressed: - logger.info("Command: resample agent state from navmesh") - if self.sim.pathfinder.is_loaded: - new_agent_state = habitat_sim.AgentState() - largest_island_ix = get_largest_island_index( - pathfinder=self.sim.pathfinder, - sim=self.sim, - allow_outdoor=False, - ) - print(f"Largest indoor island index = {largest_island_ix}") - new_agent_state.position = ( - self.sim.pathfinder.get_random_navigable_point( - island_index=largest_island_ix - ) - ) - new_agent_state.rotation = quat_from_angle_axis( - self.sim.random.uniform_float(0, 2.0 * np.pi), - np.array([0, 1, 0]), - ) - self.default_agent.set_state(new_agent_state) - else: - logger.warning( - "NavMesh is not initialized. Cannot sample new agent state." - ) - elif shift_pressed: - logger.info("Command: recompute navmesh") - self.navmesh_config_and_recompute() + if shift_pressed: + self.rec_color_mode = RecColorMode( + (self.rec_color_mode.value + 1) % len(RecColorMode) + ) + print(f"self.rec_color_mode = {self.rec_color_mode}") + self.display_receptacles = True else: - if self.sim.pathfinder.is_loaded: - self.sim.navmesh_visualization = not self.sim.navmesh_visualization - logger.info("Command: toggle navmesh") - else: - logger.warn("Warning: recompute navmesh first") + self.display_receptacles = not self.display_receptacles + print(f"self.display_receptacles = {self.display_receptacles}") # update map of moving/looking keys which are currently pressed if key in self.pressed: From 359091c831130e75c4389679b3a7fff979e5e997 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 21 Mar 2024 09:35:50 -0700 Subject: [PATCH 124/153] add region debug image output in script --- tools/check_siro_scenes.py | 64 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py index 06f98426f5..d8e177aaba 100644 --- a/tools/check_siro_scenes.py +++ b/tools/check_siro_scenes.py @@ -1,6 +1,8 @@ import os from typing import Any, Dict, List +import magnum as mn +import numpy as np from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer from habitat_sim import Simulator @@ -11,6 +13,9 @@ # from habitat_llm.dataset_generation.templated.generate_episodes import MetadataInterface +rand_colors = [mn.Color4(mn.Vector3(np.random.random(3))) for _ in range(100)] + + def get_labels_from_dict(results_dict: Dict[str, Dict[str, Any]]) -> List[str]: """ Get a list of column labels for the csv by scraping dict keys from the inner dict layers. @@ -100,6 +105,44 @@ def check_joint_popping( return unstable_aos, cumulative_errors +def draw_region_debug(sim: Simulator, region_ix: int) -> None: + """ + Draw a wireframe for the semantic region at index region_ix. + """ + region = sim.semantic_scene.regions[region_ix] + color = rand_colors[region_ix] + for edge in region.volume_edges: + sim.get_debug_line_render().draw_transformed_line( + edge[0], + edge[1], + color, + ) + + +def draw_all_regions_debug(sim: Simulator) -> None: + for reg_ix in range(len(sim.semantic_scene.regions)): + draw_region_debug(sim, reg_ix) + + +def save_region_visualizations( + sim: Simulator, out_dir: str, dbv: DebugVisualizer +) -> None: + """ + Save top-down images focused on each region with debug lines. + """ + + os.makedirs(out_dir, exist_ok=True) + + draw_all_regions_debug(sim) + dbv.peek("stage").save(output_path=os.path.join(out_dir), prefix="all_regions_") + + for rix, region in enumerate(sim.semantic_scene.regions): + draw_region_debug(sim, rix) + aabb = mn.Range3D.from_center(region.aabb.center, region.aabb.sizes / 2.0) + reg_obs = dbv._peek_bb(aabb, cam_local_pos=mn.Vector3(0, 1, 0)) + reg_obs.save(output_path=os.path.join(out_dir), prefix=f"{region.id}_") + + if __name__ == "__main__": import argparse @@ -170,5 +213,26 @@ def check_joint_popping( "unstable_aos" ] += f"{ao_handle}({joint_errors[ix]}) | " + ############################################ + # analyze and visualize regions + save_region_visualizations( + sim, os.path.join(scene_out_dir, "regions/"), dbv + ) + expected_regions = ["kitchen", "living room", "bedroom"] + all_region_cats = [ + region.category.name() for region in sim.semantic_scene.regions + ] + missing_expected_regions = [ + expected_region + for expected_region in expected_regions + if expected_region not in all_region_cats + ] + if len(missing_expected_regions) > 0: + scene_test_results[sim.curr_scene_name]["missing_expected_regions"] = "" + for expected_region in missing_expected_regions: + scene_test_results[sim.curr_scene_name][ + "missing_expected_regions" + ] += f"{expected_region} | " + csv_filepath = os.path.join(args.out_dir, "siro_scene_test_results.csv") export_results_csv(csv_filepath, scene_test_results) From bef1160954a1b40f76e7f9450b711293a6553ec6 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 21 Mar 2024 12:03:41 -0700 Subject: [PATCH 125/153] add semantic analysis from habitat-llm MEtadataInterface --- tools/check_siro_scenes.py | 53 +++++++++++++++++++++++++++++++++++--- 1 file changed, 49 insertions(+), 4 deletions(-) diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py index d8e177aaba..30dca85a09 100644 --- a/tools/check_siro_scenes.py +++ b/tools/check_siro_scenes.py @@ -1,19 +1,41 @@ import os from typing import Any, Dict, List +# NOTE: (requires habitat-lab) get metadata for semantics +import habitat.sims.habitat_simulator.sim_utilities as sutils import magnum as mn import numpy as np + +# NOTE: (requires habitat-llm) get metadata for semantics +from dataset_generation.benchmark_generation.generate_episodes import ( + MetadataInterface, + default_metadata_dict, +) from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer from habitat_sim import Simulator from habitat_sim.metadata import MetadataMediator from habitat_sim.utils.settings import default_sim_settings, make_cfg -# TODO: get metadata for semantics -# from habitat_llm.dataset_generation.templated.generate_episodes import MetadataInterface +rand_colors = [mn.Color4(mn.Vector3(np.random.random(3))) for _ in range(100)] -rand_colors = [mn.Color4(mn.Vector3(np.random.random(3))) for _ in range(100)] +def to_str_csv(data: Any) -> str: + """ + Format some data element as a string for csv such that it fits nicely into a cell. + """ + + if isinstance(data, str): + return data + if isinstance(data, (int, float)): + return str(data) + if isinstance(data, list): + list_str = "" + for elem in data: + list_str += f"{elem} |" + return list_str + + raise NotImplementedError(f"Data type {type(data)} is not supported in csv string.") def get_labels_from_dict(results_dict: Dict[str, Dict[str, Any]]) -> List[str]: @@ -46,7 +68,7 @@ def export_results_csv(filepath: str, results_dict: Dict[str, Dict[str, Any]]) - f.write(f"{scene_handle},") for label in col_labels: if label in scene_dict: - f.write(f"{scene_dict[label]},") + f.write(f"{to_str_csv(scene_dict[label])},") else: f.write(",") f.write("\n") @@ -181,6 +203,8 @@ def save_region_visualizations( mm.active_dataset = args.dataset cfg.metadata_mediator = mm + mi = MetadataInterface(default_metadata_dict) + # keyed by scene handle scene_test_results: Dict[str, Dict[str, Any]] = {} @@ -191,6 +215,8 @@ def save_region_visualizations( with Simulator(cfg) as sim: dbv = DebugVisualizer(sim) + mi.refresh_scene_caches(sim) + scene_test_results[sim.curr_scene_name] = {} scene_test_results[sim.curr_scene_name][ "ros" @@ -203,6 +229,7 @@ def save_region_visualizations( ########################################## # Check for joint popping + print(" - check joint popping") unstable_aos, joint_errors = check_joint_popping( sim, out_dir=scene_out_dir if args.save_images else None, dbv=dbv ) @@ -215,6 +242,7 @@ def save_region_visualizations( ############################################ # analyze and visualize regions + print(" - check and visualize regions") save_region_visualizations( sim, os.path.join(scene_out_dir, "regions/"), dbv ) @@ -234,5 +262,22 @@ def save_region_visualizations( "missing_expected_regions" ] += f"{expected_region} | " + ############################################## + # analyze semantics + print(" - check and visualize semantics") + scene_test_results[sim.curr_scene_name][ + "objects_missing_semantic_class" + ] = [] + missing_semantics_output = os.path.join(scene_out_dir, "missing_semantics/") + for obj in sutils.get_all_objects(sim): + if mi.get_object_instance_category(obj) is None: + scene_test_results[sim.curr_scene_name][ + "objects_missing_semantic_class" + ].append(obj.handle) + os.makedirs(missing_semantics_output, exist_ok=True) + dbv.peek(obj, peek_all_axis=True).save( + missing_semantics_output, f"{obj.handle}__" + ) + csv_filepath = os.path.join(args.out_dir, "siro_scene_test_results.csv") export_results_csv(csv_filepath, scene_test_results) From 737c060bd46ff023e22f1a7aec149dc072461f46 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 21 Mar 2024 13:43:06 -0700 Subject: [PATCH 126/153] bugfix --- examples/spot_viewer.py | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 391dfefe16..e1f15c2d2c 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -1104,7 +1104,6 @@ def navmesh_config_and_recompute(self) -> None: self.sim.recompute_navmesh(self.sim.pathfinder, self.navmesh_settings) # reset AO motion types from cache - ao_motion_types = {} for ao in ( self.sim.get_articulated_object_manager() .get_objects_by_handle_substring() From 446edd396dcea3250295d4c1bda579d2b2cafb89 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Thu, 21 Mar 2024 16:50:47 -0400 Subject: [PATCH 127/153] -- Only exit if shift is pressed when pressing i Helps being able to write the scene instance without leaving the scene, to iteratively address issues in a scene. --- examples/spot_viewer.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index e1f15c2d2c..a8bb7c8113 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -931,7 +931,11 @@ def key_press_event(self, event: Application.KeyEvent) -> None: with open("removed_clutter.txt", "a") as f: for obj_name in self.removed_clutter: f.write(obj_name + "\n") - exit() + # only exit if shift pressed + if shift_pressed: + event.accepted = True + self.exit_event(Application.ExitEvent) + return elif key == pressed.T: self.remove_outdoor_objects() @@ -1013,6 +1017,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: ray = render_camera.unproject(self.get_mouse_position(event.position)) mouse_cast_results = self.sim.cast_ray(ray=ray) if mouse_cast_results.has_hits(): + # find first non-stage object hit_idx = 0 obj_found = False while hit_idx < len(mouse_cast_results.hits) and not obj_found: From c038e3364ec5ad9af81a0aa0b1f5b1f29f940302 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Thu, 21 Mar 2024 17:28:11 -0400 Subject: [PATCH 128/153] --add func to (re)load spot; reload/reset spot for scene instance save --- examples/spot_viewer.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index a8bb7c8113..56e179fa64 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -691,6 +691,12 @@ def reconfigure_sim(self) -> None: joint_val == 0 ), "If this fails, there are non-zero joint positions in the scene_instance or default pose. Export with 'i' will clear these." + self.init_spot() + + Timer.start() + self.step = -1 + + def init_spot(self): # add the robot to the world via the wrapper robot_path = "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" agent_config = DictConfig({"articulated_agent_urdf": robot_path}) @@ -699,9 +705,6 @@ def reconfigure_sim(self) -> None: self.spot.update() self.spot_action = ExtractedBaseVelNonCylinderAction(self.sim, self.spot) - Timer.start() - self.step = -1 - def render_batch(self): """ This method updates the replay manager with the current state of environments and renders them. @@ -919,6 +922,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # with open("scene_mod_buffer.json", "w") as f: # f.write(json.dumps(self.modified_objects_buffer, indent=2)) aom = self.sim.get_articulated_object_manager() + spot_loc = self.spot.sim_obj.rigid_state aom.remove_object_by_handle(self.spot.sim_obj.handle) # clear furniture joint positions before saving @@ -936,6 +940,10 @@ def key_press_event(self, event: Application.KeyEvent) -> None: event.accepted = True self.exit_event(Application.ExitEvent) return + # rebuild spot + self.init_spot() + # put em back + self.spot.sim_obj.rigid_state = spot_loc elif key == pressed.T: self.remove_outdoor_objects() From 58b006edaffee421980ab55856510210aa386ee4 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Fri, 22 Mar 2024 16:07:20 -0400 Subject: [PATCH 129/153] --added joint state clearing from spot script. --- examples/viewer.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/examples/viewer.py b/examples/viewer.py index d6f2f7bea1..0273b62b6c 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1067,6 +1067,18 @@ def cycleScene(self, change_scene: bool, shift_pressed: bool): self.reconfigure_sim() logger.info(f"Reconfigured simulator for scene: {self.sim_settings['scene']}") + def clear_furniture_joint_states(self): + """ + Clear all furniture object joint states. + """ + for ao in ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring() + .values() + ): + j_pos = ao.joint_positions + ao.joint_positions = [0.0 for _ in range(len(j_pos))] + def key_press_event(self, event: Application.KeyEvent) -> None: """ Handles `Application.KeyEvent` on a key press by performing the corresponding functions. @@ -1141,6 +1153,9 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.print_help_text() elif key == pressed.J: + self.clear_furniture_joint_states() + + elif key == pressed.K: new_state_idx = (self.semantic_region_debug_draw_state + 1) % len( self.semantic_region_debug_draw_choices ) @@ -1798,7 +1813,8 @@ def print_help_text(self) -> None: ',': Render a Bullet collision shape debug wireframe overlay (white=active, green=sleeping, blue=wants sleeping, red=can't sleep). 'c': Run a discrete collision detection pass and render a debug wireframe overlay showing active contact points and normals (yellow=fixed length normals, red=collision distances). (+SHIFT) Toggle the contact point debug render overlay on/off. - 'j' Toggle Semantic visualization bounds (currently only Semantic Region annotations) + 'j' Clear the joint states of all articulated objects. + 'k' Toggle Semantic visualization bounds (currently only Semantic Region annotations) Object Interactions: SPACE: Toggle physics simulation on/off. From 3b5148c3e8ba7deed33e2776f4f39e36426cbcb4 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 22 Mar 2024 15:13:45 -0700 Subject: [PATCH 130/153] recompute navmesh after joint reset --- examples/spot_viewer.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 56e179fa64..4d5141d2cc 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -915,6 +915,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: elif key == pressed.J: self.clear_furniture_joint_states() + self.navmesh_config_and_recompute() elif key == pressed.I: # dump the modified object states buffer to JSON. From a7152f62b5e6dcee4c5d77309063f184f1d238a9 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Sun, 24 Mar 2024 13:40:31 -0400 Subject: [PATCH 131/153] --expand edit capabilities to provide 3 axis rotation --Toggle edit mode (displayed in viewer) using G : [move, rotate] --Cycle through edit amount (displayed in viewer) using B : translation : [0.01, 0.02, 0.05, 0.1, 0.5] m rotation : [1.0, 15.0, 45.0, 60.0, 90.0] degrees -- still use arrow keys (+alt) to edit selected object, but they have slightly different effects (listed in help) --- examples/spot_viewer.py | 213 +++++++++++++++++++++++++++++++--------- 1 file changed, 165 insertions(+), 48 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 4d5141d2cc..6a81059be8 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -8,6 +8,7 @@ import string import sys import time +from enum import Enum from typing import Any, Callable, Dict, List, Optional, Tuple flags = sys.getdlopenflags() @@ -28,6 +29,34 @@ from habitat_sim.utils.settings import default_sim_settings, make_cfg +# Describe edit type +class EditMode(Enum): + MOVE = 0 + ROTATE = 1 + NUM_VALS = 2 + + +EDIT_MODE_NAMES = ["Move object", "Rotate object"] + + +# Describe edit distance values +class DistanceMode(Enum): + VERY_SMALL = 0 + SMALL = 1 + MEDIUM = 2 + LARGE = 3 + HUGE = 4 + NUM_VALS = 5 + + +# distance values in m +DISTANCE_MODE_VALS = [0.01, 0.02, 0.05, 0.1, 0.5] +# angle value multipliers (in degrees) - multiplied by conversion +ROTATION_MULT_VALS = [1.0, 15.0, 45.0, 60.0, 90.0] +# 1 radian +BASE_EDIT_ROT_AMT = math.pi / 180.0 + + class ExtractedBaseVelNonCylinderAction: def __init__(self, sim, spot): self._sim = sim @@ -275,6 +304,15 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # variables that track app data and CPU/GPU usage self.num_frames_to_track = 60 + # Editing + # Edit mode + self.curr_edit_mode = EditMode.MOVE + # Edit distance/amount + self.curr_edit_multiplier = DistanceMode.VERY_SMALL + + # Initialize base edit changes + self.set_edit_vals() + self.previous_mouse_point = None # toggle physics simulation on/off @@ -300,8 +338,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: habitat_sim.physics.ManagedRigidObject, mn.Matrix4 ] = {} self.removed_clutter = [] - self.translation_speed = 0.05 - self.rotation_speed = math.pi / 180.0 + self.navmesh_dirty = False self.removed_objects_debug_frames = [] @@ -324,6 +361,15 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: logger.setLevel("INFO") self.print_help_text() + def set_edit_vals(self): + # Set current scene object edit values for translation and rotation + # 1 cm * multiplier + self.edit_translation_dist = DISTANCE_MODE_VALS[self.curr_edit_multiplier.value] + # 1 radian * multiplier + self.edit_rotation_amt = ( + BASE_EDIT_ROT_AMT * ROTATION_MULT_VALS[self.curr_edit_multiplier.value] + ) + def draw_removed_objects_debug_frames(self): """ Draw debug frames for all the recently removed objects. @@ -816,15 +862,6 @@ def key_press_event(self, event: Application.KeyEvent) -> None: alt_pressed = bool(event.modifiers & mod.ALT) # warning: ctrl doesn't always pass through with other key-presses - obj_translation_speed = ( - self.translation_speed - if not shift_pressed - else self.translation_speed * 2.0 - ) - obj_rotation_speed = ( - self.rotation_speed if not shift_pressed else self.rotation_speed * 4.0 - ) - if key == pressed.ESC: event.accepted = True self.exit_event(Application.ExitEvent) @@ -855,45 +892,87 @@ def key_press_event(self, event: Application.KeyEvent) -> None: logger.info(f"Command: toggle Bullet debug draw: {self.debug_bullet_draw}") elif key == pressed.LEFT: - if alt_pressed: + # if movement mode + if self.curr_edit_mode == EditMode.MOVE: self.move_selected_object( - rotation=mn.Quaternion.rotation( - mn.Rad(obj_rotation_speed), mn.Vector3.y_axis() - ) + translation=mn.Vector3.x_axis() * self.edit_translation_dist ) + # if rotation mode : rotate around y axis else: self.move_selected_object( - translation=mn.Vector3.x_axis() * obj_translation_speed + rotation=mn.Quaternion.rotation( + mn.Rad(self.edit_rotation_amt), mn.Vector3.y_axis() + ) ) elif key == pressed.RIGHT: - if alt_pressed: + # if movement mode + if self.curr_edit_mode == EditMode.MOVE: self.move_selected_object( - rotation=mn.Quaternion.rotation( - -mn.Rad(obj_rotation_speed), mn.Vector3.y_axis() - ) + translation=-mn.Vector3.x_axis() * self.edit_translation_dist ) + # if rotation mode : rotate around y axis else: self.move_selected_object( - translation=-mn.Vector3.x_axis() * obj_translation_speed + rotation=mn.Quaternion.rotation( + -mn.Rad(self.edit_rotation_amt), mn.Vector3.y_axis() + ) ) elif key == pressed.UP: - if alt_pressed: - self.move_selected_object( - translation=mn.Vector3.y_axis() * obj_translation_speed - ) + # if movement mode + if self.curr_edit_mode == EditMode.MOVE: + if alt_pressed: + self.move_selected_object( + translation=mn.Vector3.y_axis() * self.edit_translation_dist + ) + else: + self.move_selected_object( + translation=mn.Vector3.z_axis() * self.edit_translation_dist + ) + # if rotation mode : rotate around x or z axis else: - self.move_selected_object( - translation=mn.Vector3.z_axis() * obj_translation_speed - ) + if alt_pressed: + # rotate around x axis + self.move_selected_object( + rotation=mn.Quaternion.rotation( + mn.Rad(self.edit_rotation_amt), mn.Vector3.x_axis() + ) + ) + else: + # rotate around z axis + self.move_selected_object( + rotation=mn.Quaternion.rotation( + mn.Rad(self.edit_rotation_amt), mn.Vector3.z_axis() + ) + ) + elif key == pressed.DOWN: - if alt_pressed: - self.move_selected_object( - translation=-mn.Vector3.y_axis() * obj_translation_speed - ) + # if movement mode + if self.curr_edit_mode == EditMode.MOVE: + if alt_pressed: + self.move_selected_object( + translation=-mn.Vector3.y_axis() * self.edit_translation_dist + ) + else: + self.move_selected_object( + translation=-mn.Vector3.z_axis() * self.edit_translation_dist + ) + # if rotation mode : rotate around x or z axis else: - self.move_selected_object( - translation=-mn.Vector3.z_axis() * obj_translation_speed - ) + if alt_pressed: + # rotate around x axis + self.move_selected_object( + rotation=mn.Quaternion.rotation( + -mn.Rad(self.edit_rotation_amt), mn.Vector3.x_axis() + ) + ) + else: + # rotate around z axis + self.move_selected_object( + rotation=mn.Quaternion.rotation( + -mn.Rad(self.edit_rotation_amt), mn.Vector3.z_axis() + ) + ) + elif key == pressed.BACKSPACE or key == pressed.C: if self.selected_object is not None: if key == pressed.C: @@ -912,10 +991,27 @@ def key_press_event(self, event: Application.KeyEvent) -> None: ) self.selected_object = None self.navmesh_config_and_recompute() - - elif key == pressed.J: - self.clear_furniture_joint_states() - self.navmesh_config_and_recompute() + elif key == pressed.B: + # cycle through edit dist/amount multiplier + mod_val = -1 if shift_pressed else 1 + self.curr_edit_multiplier = DistanceMode( + ( + self.curr_edit_multiplier.value + + DistanceMode.NUM_VALS.value + + mod_val + ) + % DistanceMode.NUM_VALS.value + ) + # update the edit values + self.set_edit_vals() + + elif key == pressed.G: + # toggle edit mode + mod_val = -1 if shift_pressed else 1 + self.curr_edit_mode = EditMode( + (self.curr_edit_mode.value + EditMode.NUM_VALS.value + mod_val) + % EditMode.NUM_VALS.value + ) elif key == pressed.I: # dump the modified object states buffer to JSON. @@ -946,13 +1042,9 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # put em back self.spot.sim_obj.rigid_state = spot_loc - elif key == pressed.T: - self.remove_outdoor_objects() - pass - - elif key == pressed.V: - self.invert_gravity() - logger.info("Command: gravity inverted") + elif key == pressed.J: + self.clear_furniture_joint_states() + self.navmesh_config_and_recompute() elif key == pressed.N: # (default) - toggle navmesh visualization @@ -971,6 +1063,14 @@ def key_press_event(self, event: Application.KeyEvent) -> None: else: logger.warn("Warning: recompute navmesh first") + elif key == pressed.T: + self.remove_outdoor_objects() + pass + + elif key == pressed.V: + self.invert_gravity() + logger.info("Command: gravity inverted") + # update map of moving/looking keys which are currently pressed if key in self.pressed: self.pressed[key] = True @@ -1151,12 +1251,22 @@ def draw_text(self, sensor_spec): sensor_type_string = str(sensor_spec.sensor_type.name) sensor_subtype_string = str(sensor_spec.sensor_subtype.name) + edit_mode_string = EDIT_MODE_NAMES[self.curr_edit_mode.value] + + dist_mode_substr = ( + f"Translation: {self.edit_translation_dist}m" + if self.curr_edit_mode == EditMode.MOVE + else f"Rotation:{ROTATION_MULT_VALS[self.curr_edit_multiplier.value]} deg " + ) + edit_distance_mode_string = f"{dist_mode_substr}" self.window_text.render( f""" {self.fps} FPS Scene ID : {os.path.split(self.cfg.sim_cfg.scene_id)[1].split('.scene_instance')[0]} Sensor Type: {sensor_type_string} Sensor Subtype: {sensor_subtype_string} +Edit Mode: {edit_mode_string} +Edit Value: {edit_distance_mode_string} """ ) self.shader.draw(self.window_text.mesh) @@ -1193,14 +1303,21 @@ def print_help_text(self) -> None: Scene Object Modification UI: 'SHIFT+right-click': Select an object to modify. + 'G' : Change Edit mode to either Move or Rotate the selected object + 'B' (+ SHIFT) : Increment (Decrement) the current edit amounts. - With an object selected: + When Move Object mode is selected : - LEFT/RIGHT arrow keys: move the object along global X axis. - (+ALT): rotate the object around Y axis - UP/DOWN arrow keys: move the object along global Z axis. (+ALT): move the object up/down (global Y axis) + When Rotate Object mode is selected : + - LEFT/RIGHT arrow keys: rotate the object around global Y axis. + - UP/DOWN arrow keys: rotate the object around global Z axis. + (+ALT): rotate the object around global X axis. - BACKSPACE: delete the selected object - 'c': delete the selected object and record it as clutter. - 'i': save the current, modified, scene_instance file and close the viewer. Also save removed_clutter.txt containing object names of all removed clutter objects. + 'i': save the current, modified, scene_instance file. Also save removed_clutter.txt containing object names of all removed clutter objects. + - With Shift : also close the viewer. Utilities: 'r': Reset the simulator with the most recently loaded scene. From 559aac772af5e0d01dd857fe7b7a60b45d839e97 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Mon, 25 Mar 2024 09:42:46 -0400 Subject: [PATCH 132/153] --allow for different spot locations; cleanup --- examples/spot_viewer.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 6a81059be8..fa7f9be061 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -28,6 +28,11 @@ from habitat_sim.logging import LoggingContext, logger from habitat_sim.utils.settings import default_sim_settings, make_cfg +SPOT_DIR = "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" +if not os.path.isfile(SPOT_DIR): + # support other layout + SPOT_DIR = "data/scene_datasets/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" + # Describe edit type class EditMode(Enum): @@ -744,7 +749,7 @@ def reconfigure_sim(self) -> None: def init_spot(self): # add the robot to the world via the wrapper - robot_path = "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf" + robot_path = SPOT_DIR agent_config = DictConfig({"articulated_agent_urdf": robot_path}) self.spot = spot_robot.SpotRobot(agent_config, self.sim, fixed_base=True) self.spot.reconfigure() @@ -1303,8 +1308,8 @@ def print_help_text(self) -> None: Scene Object Modification UI: 'SHIFT+right-click': Select an object to modify. - 'G' : Change Edit mode to either Move or Rotate the selected object - 'B' (+ SHIFT) : Increment (Decrement) the current edit amounts. + 'g' : Change Edit mode to either Move or Rotate the selected object + 'b' (+ SHIFT) : Increment (Decrement) the current edit amounts. - With an object selected: When Move Object mode is selected : - LEFT/RIGHT arrow keys: move the object along global X axis. From f8cd0bf3dc7064f9c489ea3e7655dd1ad1c20343 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Mon, 25 Mar 2024 14:16:08 -0400 Subject: [PATCH 133/153] --enable very small movement amount --- examples/spot_viewer.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index fa7f9be061..42ebb40e2a 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -46,18 +46,19 @@ class EditMode(Enum): # Describe edit distance values class DistanceMode(Enum): - VERY_SMALL = 0 - SMALL = 1 - MEDIUM = 2 - LARGE = 3 - HUGE = 4 - NUM_VALS = 5 + TINY = 0 + VERY_SMALL = 1 + SMALL = 2 + MEDIUM = 3 + LARGE = 4 + HUGE = 5 + NUM_VALS = 6 # distance values in m -DISTANCE_MODE_VALS = [0.01, 0.02, 0.05, 0.1, 0.5] +DISTANCE_MODE_VALS = [0.001, 0.01, 0.02, 0.05, 0.1, 0.5] # angle value multipliers (in degrees) - multiplied by conversion -ROTATION_MULT_VALS = [1.0, 15.0, 45.0, 60.0, 90.0] +ROTATION_MULT_VALS = [1.0, 10.0, 30.0, 45.0, 60.0, 90.0] # 1 radian BASE_EDIT_ROT_AMT = math.pi / 180.0 From 068982b2bec1d7a7e690bba3934a730a873c6afb Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Mon, 25 Mar 2024 14:16:33 -0400 Subject: [PATCH 134/153] --clear joint vels along with joint pos --- examples/spot_viewer.py | 2 ++ examples/viewer.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 42ebb40e2a..f0ed8b5854 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -448,6 +448,8 @@ def clear_furniture_joint_states(self): if "hab_spot" not in ao.handle: j_pos = ao.joint_positions ao.joint_positions = [0.0 for _ in range(len(j_pos))] + j_vel = ao.joint_velocities + ao.joint_velocities = [0.0 for _ in range(len(j_vel))] def draw_contact_debug(self): """ diff --git a/examples/viewer.py b/examples/viewer.py index 0273b62b6c..c73add7ed3 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1078,6 +1078,8 @@ def clear_furniture_joint_states(self): ): j_pos = ao.joint_positions ao.joint_positions = [0.0 for _ in range(len(j_pos))] + j_vel = ao.joint_velocities + ao.joint_velocities = [0.0 for _ in range(len(j_vel))] def key_press_event(self, event: Application.KeyEvent) -> None: """ From 678a9325a02fcaea4cbf44997e0ebb366710f395 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 25 Mar 2024 11:18:03 -0700 Subject: [PATCH 135/153] add material correction to Blender conversion scripts for Vlad --- tools/batched_armature_to_urdf.py | 4 ++-- tools/blender_armature_to_urdf.py | 24 ++++++++++++++++++++++++ 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index 1cb36e421b..6344f2b8f9 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -72,11 +72,11 @@ def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: os.makedirs(export_path, exist_ok=True) base_command = f"blender {blend_file} --background --python {script_path} -- --export-path {export_path}" # first export the meshes - os.system(base_command + " --export-meshes") + os.system(base_command + " --export-meshes --fix-materials") # then export the URDF os.system( base_command - + " --export-ao-config --round-collision-scales --fix-collision-scales" + + " --export-urdf --export-ao-config --round-collision-scales --fix-collision-scales" ) diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py index 16ab5f902c..c407f63d8c 100644 --- a/tools/blender_armature_to_urdf.py +++ b/tools/blender_armature_to_urdf.py @@ -679,6 +679,7 @@ def export( export_urdf: bool = True, export_meshes: bool = True, export_ao_config: bool = True, + fix_materials: bool = True, **kwargs, ): """ @@ -696,6 +697,21 @@ def export( links = [] joints = [] + # fixes a gltf export error caused by 1.0 ior values + if fix_materials: + for material in bpy.data.materials: + if material.node_tree is not None: + for node in material.node_tree.nodes: + if ( + node.type == "BSDF_PRINCIPLED" + and "IOR" in node.inputs + and node.inputs["IOR"].default_value == 1.000 + ): + node.inputs["IOR"].default_value = 0.000 + print(f"Changed IOR value for material '{material.name}'") + + bpy.context.view_layer.update() + # check poll() to avoid exception. if bpy.ops.object.mode_set.poll(): bpy.ops.object.mode_set(mode="OBJECT") @@ -886,6 +902,7 @@ def export( export_ao_config = False round_collision_scales = False fix_collision_scales = False + fix_materials = False # visual shape export flags for debugging link_visuals = True @@ -971,6 +988,12 @@ def export( default=fix_collision_scales, help="Flip all negative scale elements for collision shapes.", ) + parser.add_argument( + "--fix-materials", + action="store_true", + default=fix_materials, + help="Fixes materials with ior==1.0 which cause glTF export failure.", + ) args = parser.parse_args(py_argv) export_urdf = args.export_urdf @@ -1000,6 +1023,7 @@ def export( export_urdf=export_urdf, export_meshes=export_meshes, export_ao_config=export_ao_config, + fix_materials=args.fix_materials, link_visuals=not args.no_link_visuals, collision_visuals=args.collision_visuals, joint_visuals=args.joint_visuals, From f5cee0134d98f717716b1ce11a00fa74044951a5 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 25 Mar 2024 15:35:01 -0700 Subject: [PATCH 136/153] add --no-replace option to batched converter to skip already converted assets --- tools/batched_armature_to_urdf.py | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index 6344f2b8f9..957827935c 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -80,6 +80,17 @@ def run_armature_urdf_conversion(blend_file: str, export_path: str, script_path: ) +def get_dirs_in_dir(dirpath: str) -> List[str]: + """ + Get the directory names inside a directory path. + """ + return [ + entry.split(".glb")[0] + for entry in os.listdir(dirpath) + if os.path.isdir(os.path.join(dirpath, entry)) + ] + + # ----------------------------------------- # Batches blender converter calls over a directory of blend files # e.g. python tools/batched_armature_to_urdf.py --root-dir ~/Downloads/OneDrive_1_9-27-2023/ --out-dir tools/armature_out_test/ --converter-script-path tools/blender_armature_to_urdf.py @@ -125,6 +136,13 @@ def main(): help="Substrings which indicate scenes which should be converted. Must be provided with a scene map file. When provided, only these scenes are converted.", default=None, ) + parser.add_argument( + "--no-replace", + default=False, + action="store_true", + help="If specified, cull candidate .blend files if there already exists a matching output directory for the asset.", + ) + args = parser.parse_args() root_dir = args.root_dir assert os.path.isdir(root_dir), "directory must exist." @@ -143,6 +161,19 @@ def main(): ] blend_paths = list(set(blend_paths) - set(skipped_strings)) + if args.no_replace: + out_dir_dirs = get_dirs_in_dir(args.out_dir) + remaining_blend_paths = [ + blend + for blend in blend_paths + if blend.split("/")[-1].split(".")[0] not in out_dir_dirs + ] + print(f"original blends = {len(blend_paths)}") + print(f"existing dirs = {len(out_dir_dirs)}") + print(f"remaining_blend_paths = {len(remaining_blend_paths)}") + print(f"remaining_blend_paths = {remaining_blend_paths}") + blend_paths = remaining_blend_paths + if args.scene_map_file is not None and args.scenes is not None: # load the scene map file and limit the object set by scenes scene_object_map = load_scene_map_file(args.scene_map_file) From d6bb457eb5465382de063e3c730e59b23b4a5464 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 25 Mar 2024 20:20:49 -0700 Subject: [PATCH 137/153] update no-replace to check for expected contents instead of only a directory --- tools/batched_armature_to_urdf.py | 33 +++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index 957827935c..8d182fdc0f 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -91,6 +91,29 @@ def get_dirs_in_dir(dirpath: str) -> List[str]: ] +def get_dirs_in_dir_complete(dirpath: str) -> List[str]: + """ + Get the directory names inside a directory path for directories which contain: + - urdf + - ao_config.json + - at least 2 .glb files (for articulation) + TODO: check the urdf contents for all .glbs + """ + relevant_entries = [] + for entry in os.listdir(dirpath): + entry_path = os.path.join(dirpath, entry) + entry_name = entry.split(".glb")[0] + if os.path.isdir(entry_path): + contents = os.listdir(entry_path) + urdfs = [file for file in contents if file.endswith(".urdf")] + configs = [file for file in contents if file.endswith(".ao_config.json")] + glbs = [file for file in contents if file.endswith(".glb")] + if len(urdfs) > 0 and len(configs) > 0 and len(glbs) > 2: + relevant_entries.append(entry_name) + + return relevant_entries + + # ----------------------------------------- # Batches blender converter calls over a directory of blend files # e.g. python tools/batched_armature_to_urdf.py --root-dir ~/Downloads/OneDrive_1_9-27-2023/ --out-dir tools/armature_out_test/ --converter-script-path tools/blender_armature_to_urdf.py @@ -162,7 +185,8 @@ def main(): blend_paths = list(set(blend_paths) - set(skipped_strings)) if args.no_replace: - out_dir_dirs = get_dirs_in_dir(args.out_dir) + # out_dir_dirs = get_dirs_in_dir(args.out_dir) + out_dir_dirs = get_dirs_in_dir_complete(args.out_dir) remaining_blend_paths = [ blend for blend in blend_paths @@ -171,8 +195,13 @@ def main(): print(f"original blends = {len(blend_paths)}") print(f"existing dirs = {len(out_dir_dirs)}") print(f"remaining_blend_paths = {len(remaining_blend_paths)}") - print(f"remaining_blend_paths = {remaining_blend_paths}") + remaining_hashes = [ + blend_path.split("/")[-1] for blend_path in remaining_blend_paths + ] + print(f"remaining_hashes = {remaining_hashes}") blend_paths = remaining_blend_paths + # use this to check, but not commit to trying again + # exit() if args.scene_map_file is not None and args.scenes is not None: # load the scene map file and limit the object set by scenes From 86274adf96fe039e204d60e59441366f7ce5ed2a Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 26 Mar 2024 09:36:00 -0700 Subject: [PATCH 138/153] add ao smoke test and correct receptacle check for ao converter validation script --- tools/check_siro_aos.py | 223 ++++++++++++++++++ tools/generate_blend_to_urdf_parser_report.py | 2 +- 2 files changed, 224 insertions(+), 1 deletion(-) create mode 100644 tools/check_siro_aos.py diff --git a/tools/check_siro_aos.py b/tools/check_siro_aos.py new file mode 100644 index 0000000000..33c02307de --- /dev/null +++ b/tools/check_siro_aos.py @@ -0,0 +1,223 @@ +import os +from typing import Any, Dict, List + +# NOTE: (requires habitat-lab) get metadata for semantics +import magnum as mn +import numpy as np +from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer + +from habitat_sim import Simulator +from habitat_sim.metadata import MetadataMediator +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +rand_colors = [mn.Color4(mn.Vector3(np.random.random(3))) for _ in range(100)] + + +def to_str_csv(data: Any) -> str: + """ + Format some data element as a string for csv such that it fits nicely into a cell. + """ + if data is None: + return "None" + if isinstance(data, str): + return data + if isinstance(data, (int, float)): + return str(data) + if isinstance(data, list): + list_str = "" + for elem in data: + list_str += f"{elem} |" + return list_str + + raise NotImplementedError(f"Data type {type(data)} is not supported in csv string.") + + +def get_labels_from_dict(results_dict: Dict[str, Dict[str, Any]]) -> List[str]: + """ + Get a list of column labels for the csv by scraping dict keys from the inner dict layers. + """ + labels = [] + for ao_dict in results_dict.values(): + for dict_key in ao_dict: + if dict_key not in labels: + labels.append(dict_key) + return labels + + +def export_results_csv(filepath: str, results_dict: Dict[str, Dict[str, Any]]) -> None: + assert filepath.endswith(".csv") + + col_labels = get_labels_from_dict(results_dict) + + with open(filepath, "w") as f: + # first write the column labels + f.write("ao,") + for c_label in col_labels: + f.write(f"{c_label},") + f.write("\n") + + # now a row for each scene + for ao_handle, ao_dict in results_dict.items(): + # write the ao handle column + f.write(f"{ao_handle},") + for label in col_labels: + if label in ao_dict: + f.write(f"{to_str_csv(ao_dict[label])},") + else: + f.write(",") + f.write("\n") + print(f"Wrote results csv to {filepath}") + + +def check_joint_popping( + sim: Simulator, out_dir: str = None, dbv: DebugVisualizer = None +) -> List[str]: + """ + Get a list of ao handles for objects which are not stable during simulation. + Checks the initial joint state, then simulates 1 second, then check the joint state again. Changes indicate popping, collisions, loose hinges, or other instability. + + :param out_dir: If provided, save debug images to the output directory prefixed "joint_pop____". + """ + + if out_dir is not None and dbv is None: + dbv = DebugVisualizer(sim) + + # record the ao handles + unstable_aos = [] + # record the sum of errors across all joints + cumulative_errors = [] + + ao_initial_joint_states = {} + + for ao_handle, ao in ( + sim.get_articulated_object_manager().get_objects_by_handle_substring().items() + ): + ao_initial_joint_states[ao_handle] = ao.joint_positions + + sim.step_physics(2.0) + + # cumulative error must be above this threshold to count as "unstable" + eps = 1e-3 + + for ao_handle, ao in ( + sim.get_articulated_object_manager().get_objects_by_handle_substring().items() + ): + jp = ao.joint_positions + if ao_initial_joint_states[ao_handle] != jp: + cumulative_error = sum( + [ + abs(ao_initial_joint_states[ao_handle][i] - jp[i]) + for i in range(len(jp)) + ] + ) + if cumulative_error > eps: + cumulative_errors.append(cumulative_error) + unstable_aos.append(ao_handle) + if out_dir is not None: + dbv.peek(ao_handle, peek_all_axis=True).save( + output_path=out_dir, prefix=f"joint_pop__{ao_handle}__" + ) + + return unstable_aos, cumulative_errors + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + + parser.add_argument( + "--dataset", + default="default", + type=str, + metavar="DATASET", + help='dataset configuration file to use (default: "default")', + ) + parser.add_argument( + "--out-dir", + default="siro_test_results/", + type=str, + help="directory in which to cache images and results csv.", + ) + parser.add_argument( + "--save-images", + default=False, + action="store_true", + help="save images during tests into the output directory.", + ) + args = parser.parse_args() + + os.makedirs(args.out_dir, exist_ok=True) + + # create an initial simulator config + sim_settings: Dict[str, Any] = default_sim_settings + sim_settings["scene_dataset_config_file"] = args.dataset + cfg = make_cfg(sim_settings) + + # pre-initialize a MetadataMediator to iterate over scenes + mm = MetadataMediator() + mm.active_dataset = args.dataset + cfg.metadata_mediator = mm + + # keyed by ao handle + ao_test_results: Dict[str, Dict[str, Any]] = {} + + ao_ix = 0 + # split up the load per-simulator reconfigure to balance memory overhead with init time + iters_per_sim = 50 + ao_handles = mm.ao_template_manager.get_template_handles() + while ao_ix < len(ao_handles): + with Simulator(cfg) as sim: + dbv = DebugVisualizer(sim) + aom = sim.get_articulated_object_manager() + + for _i in range(iters_per_sim): + if ao_ix >= len(ao_handles): + # early escape if done + break + + ao_handle = ao_handles[ao_ix] + ao_short_handle = ao_handle.split("/")[-1].split(".")[0] + ao_ix += 1 + ao_test_results[ao_short_handle] = {} + asset_failure_message = None + ao = None + + # first try to load the asset + try: + ao = aom.add_articulated_object_by_template_handle(ao_handle) + except Exception as e: + print(f"Failed to load asset {ao_handle}. '{repr(e)}'") + asset_failure_message = repr(e) + + if ao is None: + # load failed, record the message and continue + ao_test_results[ao_short_handle]["failure_log"] = to_str_csv( + asset_failure_message + ) + continue + + # check joint popping + unstable_aos, joint_errors = check_joint_popping( + sim, out_dir=args.out_dir if args.save_images else None, dbv=dbv + ) + + if len(unstable_aos) > 0: + ao_test_results[ao_short_handle][ + "joint_popping_error" + ] = joint_errors[0] + + ########################################### + # produce a gif of actuation + # TODO: + + ############################################# + # DONE: clear the scene for next iteration + aom.remove_all_objects() + + # check if done with last ao + if ao_ix >= len(ao_handles): + break + + csv_filepath = os.path.join(args.out_dir, "siro_ao_test_results.csv") + export_results_csv(csv_filepath, ao_test_results) diff --git a/tools/generate_blend_to_urdf_parser_report.py b/tools/generate_blend_to_urdf_parser_report.py index 16f9598895..422928c775 100644 --- a/tools/generate_blend_to_urdf_parser_report.py +++ b/tools/generate_blend_to_urdf_parser_report.py @@ -169,7 +169,7 @@ def main(): # NOTE: there could be missing assets here, but without parsing the blend file again, we wouldn't know. Heuristic is to expect at least one. num_rec_meshes = len( - find_files(folder_path, file_endswith, "_receptacle.glb") + find_files(folder_path, file_endswith, "_receptacle_mesh.glb") ) one_receptacle_exists = num_rec_meshes > 0 From d24fa0da345f75305364be3ca9f0d5c9c55511b6 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 26 Mar 2024 15:48:50 -0700 Subject: [PATCH 139/153] update URDF migration script to be more configurable from CLI --- ...place_articulated_models_in_rigid_scene.py | 38 +++++++++++++------ 1 file changed, 27 insertions(+), 11 deletions(-) diff --git a/tools/replace_articulated_models_in_rigid_scene.py b/tools/replace_articulated_models_in_rigid_scene.py index 7216f51fd8..4f4ed380d6 100644 --- a/tools/replace_articulated_models_in_rigid_scene.py +++ b/tools/replace_articulated_models_in_rigid_scene.py @@ -52,7 +52,11 @@ def find_files(root_dir: str, discriminator: Callable[[str], bool]) -> List[str] def find_and_replace_articulated_models_for_config( - filepath: str, top_dir: str, urdf_names: str + filepath: str, + top_dir: str, + urdf_names: str, + src_dir: str = "scenes-uncluttered", + dest_dir: str = "scenes-articulated-uncluttered", ) -> None: """ For a given scene config, try to find a matching articulated objects for each rigid object. If found, add them to the config, replacing the rigid objects. @@ -109,11 +113,7 @@ def find_and_replace_articulated_models_for_config( scene_conf["articulated_object_instances"] = ao_instance_data if file_is_modified: - filepath = ( - filepath.split("scenes-uncluttered")[0] - + "scenes-articulated-uncluttered" - + filepath.split("scenes-uncluttered")[-1] - ) + filepath = filepath.split(src_dir)[0] + dest_dir + filepath.split(src_dir)[-1] with open(filepath, "w") as f: json.dump(scene_conf, f) @@ -127,6 +127,18 @@ def main(): type=str, help="path to HSSD SceneDataset root directory containing 'fphab-uncluttered.scene_dataset_config.json'.", ) + parser.add_argument( + "--src-dir", + type=str, + default="scenes-uncluttered", + help="Name of the source scene config directory within root-dir.", + ) + parser.add_argument( + "--dest-dir", + type=str, + default="scenes-articulated-uncluttered", + help="Name of the destination scene config directory within root-dir. Will be created if doesn't exist.", + ) parser.add_argument( "--scenes", nargs="+", @@ -136,15 +148,15 @@ def main(): ) args = parser.parse_args() fp_root_dir = args.dataset_root_dir - config_root_dir = os.path.join(fp_root_dir, "scenes-uncluttered") + src_dir = args.src_dir + dest_dir = args.dest_dir + config_root_dir = os.path.join(fp_root_dir, src_dir) configs = find_files(config_root_dir, file_is_scene_config) urdf_dir = os.path.join(fp_root_dir, "urdf/") urdf_files = find_files(urdf_dir, file_is_urdf) # create scene output directory - os.makedirs( - os.path.join(fp_root_dir, "scenes-articulated-uncluttered"), exist_ok=True - ) + os.makedirs(os.path.join(fp_root_dir, dest_dir), exist_ok=True) invalid_urdf_files = [] @@ -186,7 +198,11 @@ def urdf_has_meshes_and_config(urdf_filepath: str) -> bool: for _ix, filepath in enumerate(configs): find_and_replace_articulated_models_for_config( - filepath, urdf_names=urdf_names, top_dir=fp_root_dir + filepath, + urdf_names=urdf_names, + top_dir=fp_root_dir, + src_dir=src_dir, + dest_dir=dest_dir, ) print( From aeaf15558e4f8e098660cc2f854cd179de3b8e8e Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Tue, 26 Mar 2024 12:28:23 -0400 Subject: [PATCH 140/153] --increase radius of receptacle view --- examples/viewer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index c73add7ed3..e78d41ee70 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -723,9 +723,9 @@ def debug_draw(self): if ix > 0: r_pos = rec_obj.transformation.transform_point(key_point) c_to_r = r_pos - c_pos - # only display receptacles within 4 meters centered in view + # only display receptacles within 8 meters centered in view if ( - c_to_r.length() < 4 + c_to_r.length() < 8 and mn.math.dot((c_to_r).normalized(), c_forward) > 0.7 ): in_view = True From b9c050473546d27927ff26d268cccbe6fd352f63 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Wed, 27 Mar 2024 14:37:57 -0400 Subject: [PATCH 141/153] --temp mechanism to save scene instance and exit. just uncomment --- examples/viewer.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/viewer.py b/examples/viewer.py index e78d41ee70..a7a085f558 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -958,6 +958,10 @@ def reconfigure_sim( self.tiled_sims[i].config.sim_cfg.scene_id = "NONE" self.tiled_sims[i].reconfigure(self.cfg) + # #resave scene instance + # self.sim.save_current_scene_config(overwrite=True) + # sys. exit() + # post reconfigure self.default_agent = self.sim.get_agent(self.agent_id) self.render_camera = self.default_agent.scene_node.node_sensor_suite.get( From 0bce732b3409b3770587c955b66e69bb2cee3cbc Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 28 Mar 2024 13:07:45 -0700 Subject: [PATCH 142/153] add option to restrict batch processing to one or a set of assets with CLI --- tools/batched_armature_to_urdf.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py index 8d182fdc0f..25a326c7c0 100644 --- a/tools/batched_armature_to_urdf.py +++ b/tools/batched_armature_to_urdf.py @@ -165,6 +165,13 @@ def main(): action="store_true", help="If specified, cull candidate .blend files if there already exists a matching output directory for the asset.", ) + parser.add_argument( + "--assets", + nargs="+", + type=str, + help="Asset name substrings which indicate the subset of assets which should be converted. When provided, only these assets are converted.", + default=None, + ) args = parser.parse_args() root_dir = args.root_dir @@ -214,6 +221,12 @@ def main(): limited_object_paths.append(blend_path) blend_paths = list(set(limited_object_paths)) + if args.assets is not None: + asset_blend_paths = [] + for name_str in args.assets: + asset_blend_paths.extend([path for path in blend_paths if name_str in path]) + blend_paths = asset_blend_paths + for blend_path in blend_paths: run_armature_urdf_conversion( blend_file=blend_path, From 96fe216b65c233c3e1881c1287209a61c7be4910 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Thu, 28 Mar 2024 16:07:46 -0700 Subject: [PATCH 143/153] make output json human readable --- tools/replace_articulated_models_in_rigid_scene.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/replace_articulated_models_in_rigid_scene.py b/tools/replace_articulated_models_in_rigid_scene.py index 4f4ed380d6..e8e87622d4 100644 --- a/tools/replace_articulated_models_in_rigid_scene.py +++ b/tools/replace_articulated_models_in_rigid_scene.py @@ -115,7 +115,7 @@ def find_and_replace_articulated_models_for_config( if file_is_modified: filepath = filepath.split(src_dir)[0] + dest_dir + filepath.split(src_dir)[-1] with open(filepath, "w") as f: - json.dump(scene_conf, f) + json.dump(scene_conf, f, indent=4) def main(): From e48ec67c91c4335056492559adfed49e8a3a8adf Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Fri, 29 Mar 2024 10:42:54 -0400 Subject: [PATCH 144/153] --add undo function : Return current selected obj to orig state From when first selected --- examples/spot_viewer.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index f0ed8b5854..edd2434619 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -338,6 +338,7 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # object selection and manipulation interface self.selected_object = None + self.selected_object_orig_transform = mn.Matrix4().identity_init() self.last_hit_details = None # cache modified states of any objects moved by the interface. self.modified_objects_buffer: Dict[ @@ -1075,6 +1076,22 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.remove_outdoor_objects() pass + elif key == pressed.U: + # if an object is selected, restore its last transformation state - UNDO of edits since last selected + print("Undo selected") + if self.selected_object is not None: + print( + f"Sel Obj : {self.selected_object.handle} : Current object transformation : \n{self.selected_object.transformation}\n Being replaced by saved transformation : \n{self.selected_object.transformation}" + ) + orig_mt = self.selected_object.motion_type + self.selected_object.motion_type = ( + habitat_sim.physics.MotionType.KINEMATIC + ) + self.selected_object.transformation = ( + self.selected_object_orig_transform + ) + self.selected_object.motion_type = orig_mt + elif key == pressed.V: self.invert_gravity() logger.info("Command: gravity inverted") @@ -1151,6 +1168,11 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: ) else: print("This is the stage.") + # record current selected object's transformation, to restore if undo is pressed + if self.selected_object is not None: + self.selected_object_orig_transform = ( + self.selected_object.transformation + ) self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() From 583b14dbe750b06fbc26d667138ef5141fd8274d Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 29 Mar 2024 10:23:53 -0700 Subject: [PATCH 145/153] add AO import to spot_viewer with 'v'. Add ao category sorting and visualization to check_siro_aos.py --- examples/spot_viewer.py | 26 +++++++++++++++++++++++-- tools/check_siro_aos.py | 42 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 66 insertions(+), 2 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index edd2434619..1da032485a 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -1093,8 +1093,30 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.selected_object.motion_type = orig_mt elif key == pressed.V: - self.invert_gravity() - logger.info("Command: gravity inverted") + # inject a new AO by handle substring in front of the agent + + # get user input + ao_substring = input( + "Load ArticulatedObject. Enter an AO handle substring, first match will be added:" + ).strip() + + aotm = self.sim.metadata_mediator.ao_template_manager + aom = self.sim.get_articulated_object_manager() + ao_handles = aotm.get_template_handles(ao_substring) + if len(ao_handles) == 0: + print(f"No AO found matching substring: '{ao_substring}'") + elif len(ao_handles) > 1: + print(f"Multiple AOs found matching substring: '{ao_substring}'.") + matching_ao_handle = ao_handles[0] + print(f"Adding AO: '{matching_ao_handle}'") + aot = aotm.get_template_by_handle(matching_ao_handle) + aot.base_type = "FIXED" + aotm.register_template(aot) + ao = aom.add_articulated_object_by_template_handle(matching_ao_handle) + in_front_of_spot = self.spot.base_transformation.transform_point( + [1.5, 0.0, 0.0] + ) + ao.translation = in_front_of_spot # update map of moving/looking keys which are currently pressed if key in self.pressed: diff --git a/tools/check_siro_aos.py b/tools/check_siro_aos.py index 33c02307de..1344948266 100644 --- a/tools/check_siro_aos.py +++ b/tools/check_siro_aos.py @@ -4,10 +4,19 @@ # NOTE: (requires habitat-lab) get metadata for semantics import magnum as mn import numpy as np + +# NOTE: (requires habitat-llm) get metadata for semantics +from dataset_generation.benchmark_generation.generate_episodes import ( + MetadataInterface, + default_metadata_dict, + object_hash_from_handle, +) +from habitat.datasets.rearrange.samplers.receptacle import find_receptacles from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer from habitat_sim import Simulator from habitat_sim.metadata import MetadataMediator +from habitat_sim.physics import ManagedArticulatedObject from habitat_sim.utils.settings import default_sim_settings, make_cfg rand_colors = [mn.Color4(mn.Vector3(np.random.random(3))) for _ in range(100)] @@ -121,6 +130,16 @@ def check_joint_popping( return unstable_aos, cumulative_errors +def recompute_ao_bbs(ao: ManagedArticulatedObject) -> None: + """ + Recomputes the link SceneNode bounding boxes for all ao links. + NOTE: Gets around an observed loading bug. Call before trying to peek an AO. + """ + for link_ix in range(-1, ao.num_links): + link_node = ao.get_link_scene_node(link_ix) + link_node.compute_cumulative_bb() + + if __name__ == "__main__": import argparse @@ -159,6 +178,8 @@ def check_joint_popping( mm.active_dataset = args.dataset cfg.metadata_mediator = mm + mi = MetadataInterface(default_metadata_dict) + # keyed by ao handle ao_test_results: Dict[str, Dict[str, Any]] = {} @@ -211,6 +232,27 @@ def check_joint_popping( # produce a gif of actuation # TODO: + ########################################### + # load the receptacles + try: + recs = find_receptacles(sim) + except Exception as e: + print(f"Failed to load receptacles for {ao_handle}. '{repr(e)}'") + asset_failure_message = repr(e) + + ########################################### + # snap an image and sort into category subfolder + recompute_ao_bbs(ao) + hash_name = object_hash_from_handle(ao_handle) + cat = mi.get_object_category(hash_name) + if cat is None: + cat = "None" + + ao_peek = dbv.peek(ao.handle, peek_all_axis=True) + cat_dir = os.path.join(args.out_dir, f"ao_categories/{cat}/") + os.makedirs(cat_dir, exist_ok=True) + ao_peek.save(cat_dir, prefix=hash_name) + ############################################# # DONE: clear the scene for next iteration aom.remove_all_objects() From 2df77716d38bb316fdef866c5e439430e9210d06 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 29 Mar 2024 15:34:52 -0700 Subject: [PATCH 146/153] cleanup ao script and add joint opener to spot_viewer --- examples/spot_viewer.py | 38 ++++++++++++++++++++++++++++++++------ tools/check_siro_aos.py | 5 ++++- 2 files changed, 36 insertions(+), 7 deletions(-) diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py index 1da032485a..0a8d28c85a 100644 --- a/examples/spot_viewer.py +++ b/examples/spot_viewer.py @@ -187,6 +187,16 @@ def step(self, forward, lateral, angular): self.update_base(ang_vel != 0.0) +def recompute_ao_bbs(ao: habitat_sim.physics.ManagedArticulatedObject) -> None: + """ + Recomputes the link SceneNode bounding boxes for all ao links. + NOTE: Gets around an observed loading bug. Call before trying to peek an AO. + """ + for link_ix in range(-1, ao.num_links): + link_node = ao.get_link_scene_node(link_ix) + link_node.compute_cumulative_bb() + + class HabitatSimInteractiveViewer(Application): # the maximum number of chars displayable in the app window # using the magnum text module. These chars are used to @@ -1052,8 +1062,19 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.spot.sim_obj.rigid_state = spot_loc elif key == pressed.J: - self.clear_furniture_joint_states() - self.navmesh_config_and_recompute() + if shift_pressed and isinstance( + self.selected_object, habitat_sim.physics.ManagedArticulatedObject + ): + # open the selected receptacle + for link_ix in self.selected_object.get_link_ids(): + if self.selected_object.get_link_joint_type(link_ix) in [ + habitat_sim.physics.JointType.Prismatic, + habitat_sim.physics.JointType.Revolute, + ]: + sutils.open_link(self.selected_object, link_ix) + else: + self.clear_furniture_joint_states() + self.navmesh_config_and_recompute() elif key == pressed.N: # (default) - toggle navmesh visualization @@ -1105,6 +1126,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: ao_handles = aotm.get_template_handles(ao_substring) if len(ao_handles) == 0: print(f"No AO found matching substring: '{ao_substring}'") + return elif len(ao_handles) > 1: print(f"Multiple AOs found matching substring: '{ao_substring}'.") matching_ao_handle = ao_handles[0] @@ -1113,10 +1135,14 @@ def key_press_event(self, event: Application.KeyEvent) -> None: aot.base_type = "FIXED" aotm.register_template(aot) ao = aom.add_articulated_object_by_template_handle(matching_ao_handle) - in_front_of_spot = self.spot.base_transformation.transform_point( - [1.5, 0.0, 0.0] - ) - ao.translation = in_front_of_spot + if ao is not None: + recompute_ao_bbs(ao) + in_front_of_spot = self.spot.base_transformation.transform_point( + [1.5, 0.0, 0.0] + ) + ao.translation = in_front_of_spot + else: + print("Failed to load AO.") # update map of moving/looking keys which are currently pressed if key in self.pressed: diff --git a/tools/check_siro_aos.py b/tools/check_siro_aos.py index 1344948266..bca4cfed0e 100644 --- a/tools/check_siro_aos.py +++ b/tools/check_siro_aos.py @@ -239,6 +239,9 @@ def recompute_ao_bbs(ao: ManagedArticulatedObject) -> None: except Exception as e: print(f"Failed to load receptacles for {ao_handle}. '{repr(e)}'") asset_failure_message = repr(e) + ao_test_results[ao_short_handle]["failure_log"] = to_str_csv( + asset_failure_message + ) ########################################### # snap an image and sort into category subfolder @@ -251,7 +254,7 @@ def recompute_ao_bbs(ao: ManagedArticulatedObject) -> None: ao_peek = dbv.peek(ao.handle, peek_all_axis=True) cat_dir = os.path.join(args.out_dir, f"ao_categories/{cat}/") os.makedirs(cat_dir, exist_ok=True) - ao_peek.save(cat_dir, prefix=hash_name) + ao_peek.save(cat_dir, prefix=hash_name + "__") ############################################# # DONE: clear the scene for next iteration From b5f26d656b15f8c43752fb7fcd9ce93499b38810 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 1 Apr 2024 15:17:34 -0700 Subject: [PATCH 147/153] minor change to naming of region files --- tools/check_siro_scenes.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py index 30dca85a09..182dadbdca 100644 --- a/tools/check_siro_scenes.py +++ b/tools/check_siro_scenes.py @@ -159,10 +159,13 @@ def save_region_visualizations( dbv.peek("stage").save(output_path=os.path.join(out_dir), prefix="all_regions_") for rix, region in enumerate(sim.semantic_scene.regions): + normalized_region_id = region.id.replace("/", "|").replace(" ", "_") draw_region_debug(sim, rix) aabb = mn.Range3D.from_center(region.aabb.center, region.aabb.sizes / 2.0) reg_obs = dbv._peek_bb(aabb, cam_local_pos=mn.Vector3(0, 1, 0)) - reg_obs.save(output_path=os.path.join(out_dir), prefix=f"{region.id}_") + reg_obs.save( + output_path=os.path.join(out_dir), prefix=f"{normalized_region_id}_" + ) if __name__ == "__main__": From c858875749f378a435ee93326683c5e981c29f23 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 1 Apr 2024 15:39:40 -0700 Subject: [PATCH 148/153] add region count to siro scene check script --- tools/check_siro_scenes.py | 65 ++++++++++++++++++++++++++++++++------ 1 file changed, 56 insertions(+), 9 deletions(-) diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py index 182dadbdca..a0d52e8b01 100644 --- a/tools/check_siro_scenes.py +++ b/tools/check_siro_scenes.py @@ -1,4 +1,5 @@ import os +from collections import defaultdict from typing import Any, Dict, List # NOTE: (requires habitat-lab) get metadata for semantics @@ -168,6 +169,32 @@ def save_region_visualizations( ) +def get_region_counts(sim: Simulator) -> Dict[str, int]: + """ + Count all the region categories in the active scene. + """ + + region_counts = defaultdict(lambda: 0) + for region in sim.semantic_scene.regions: + region_counts[region.category.name()] += 1 + return region_counts + + +def save_region_counts_csv(region_counts: Dict[str, int], filepath: str) -> None: + """ + Save the region counts to a csv file. + """ + + assert filepath.endswith(".csv") + + with open(filepath, "w") as f: + f.write("region_name, count\n") + for region_name, count in region_counts.items(): + f.write(f"{region_name}, {count}, \n") + + print(f"Wrote region counts csv to {filepath}") + + if __name__ == "__main__": import argparse @@ -211,10 +238,19 @@ def save_region_visualizations( # keyed by scene handle scene_test_results: Dict[str, Dict[str, Any]] = {} + # count all region category names in all scenes + region_counts: Dict[str, int] = defaultdict(lambda: 0) + + num_scenes = len(mm.get_scene_handles()) + # for each scene, initialize a fresh simulator and run tests - for scene_handle in mm.get_scene_handles(): - print(f"Setting up scene for {scene_handle}") + for s_ix, scene_handle in enumerate(mm.get_scene_handles()): + print("=================================================================") + print( + f"Setting up scene for {scene_handle} ({s_ix}|{num_scenes} = {s_ix/float(num_scenes)*100}%)" + ) cfg.sim_cfg.scene_id = scene_handle + print(" - init") with Simulator(cfg) as sim: dbv = DebugVisualizer(sim) @@ -230,6 +266,13 @@ def save_region_visualizations( scene_out_dir = os.path.join(args.out_dir, f"{sim.curr_scene_name}/") + ########################################## + # Check region counts + print(" - region counts") + scene_region_counts = get_region_counts(sim) + for region_name, count in scene_region_counts.items(): + region_counts[region_name] += count + ########################################## # Check for joint popping print(" - check joint popping") @@ -246,9 +289,10 @@ def save_region_visualizations( ############################################ # analyze and visualize regions print(" - check and visualize regions") - save_region_visualizations( - sim, os.path.join(scene_out_dir, "regions/"), dbv - ) + if args.save_images: + save_region_visualizations( + sim, os.path.join(scene_out_dir, "regions/"), dbv + ) expected_regions = ["kitchen", "living room", "bedroom"] all_region_cats = [ region.category.name() for region in sim.semantic_scene.regions @@ -277,10 +321,13 @@ def save_region_visualizations( scene_test_results[sim.curr_scene_name][ "objects_missing_semantic_class" ].append(obj.handle) - os.makedirs(missing_semantics_output, exist_ok=True) - dbv.peek(obj, peek_all_axis=True).save( - missing_semantics_output, f"{obj.handle}__" - ) + if args.save_images: + os.makedirs(missing_semantics_output, exist_ok=True) + dbv.peek(obj, peek_all_axis=True).save( + missing_semantics_output, f"{obj.handle}__" + ) csv_filepath = os.path.join(args.out_dir, "siro_scene_test_results.csv") export_results_csv(csv_filepath, scene_test_results) + region_count_csv_filepath = os.path.join(args.out_dir, "region_counts.csv") + save_region_counts_csv(region_counts, region_count_csv_filepath) From 2ac033d45439ae96747fe4ee94b6b3cecefd29a7 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 5 Apr 2024 09:45:09 -0700 Subject: [PATCH 149/153] default link logic, receptacle filtering, refactor --- examples/viewer.py | 235 ++++++++++++++++++++++++---- tools/check_siro_scenes.py | 312 ++++++++++++++++++++++++++++++++++++- 2 files changed, 511 insertions(+), 36 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index a7a085f558..9738447e47 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -19,8 +19,12 @@ import habitat.sims.habitat_simulator.sim_utilities as sutils import magnum as mn import numpy as np -from habitat.datasets.rearrange.navmesh_utils import get_largest_island_index +from habitat.datasets.rearrange.navmesh_utils import ( + get_largest_island_index, + unoccluded_navmesh_snap, +) from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler +from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer from magnum import shaders, text from magnum.platform.glfw import Application @@ -280,6 +284,9 @@ def __init__( self.selected_rec = None self.ao_link_map = None + # index of the largest indoor island + self.largest_island_ix = -1 + # Sim reconfigure self.reconfigure_sim(mm) # load appropriate filter file for scene @@ -307,6 +314,7 @@ def __init__( self.sim.metadata_mediator.object_template_manager.load_configs( "data/objects/ycb/configs/" ) + self.initialize_clutter_object_set() # ----------------------------------------- # compute NavMesh if not already loaded by the scene. @@ -382,15 +390,6 @@ def load_scene_filter_file(self): f"WARNING: No rec filter file configured for scene {self.sim.curr_scene_name}." ) - def get_rec_instance_name(self, receptacle: hab_receptacle.Receptacle) -> str: - """ - Gets a unique string name for the Receptacle instance. - Multiple Receptacles can share a name (e.g. if the object has multiple instances in the scene). - The unique name is constructed as '|'. - """ - rec_unique_name = receptacle.parent_object_handle + "|" + receptacle.name - return rec_unique_name - def get_closest_tri_receptacle( self, pos: mn.Vector3, max_dist: float = 3.5 ) -> Optional[hab_receptacle.TriangleMeshReceptacle]: @@ -457,7 +456,7 @@ def compute_rec_filter_state( } for rec in self.receptacles: - rec_unique_name = self.get_rec_instance_name(rec) + rec_unique_name = rec.unique_name # respect already marked receptacles if rec_unique_name not in self.rec_filter_data["manually_filtered"]: rec_dat = self._cpo.gt_data[self.rec_to_poh[rec]]["receptacles"][ @@ -657,7 +656,7 @@ def debug_draw(self): ) ) for receptacle in self.receptacles: - rec_unique_name = self.get_rec_instance_name(receptacle) + rec_unique_name = receptacle.unique_name # filter all non-active receptacles if ( self.rec_filter_data is not None @@ -755,7 +754,8 @@ def debug_draw(self): ): rec_color = mn.Color4.magenta() elif rec_unique_name in self.rec_filter_data["height_filtered"]: - rec_color = mn.Color4.blue() + # orange + rec_color = mn.Color4(1.0, 0.66, 0.0, 1.0) elif ( self.cpo_initialized and self.rec_color_mode != RecColorMode.DEFAULT @@ -919,6 +919,23 @@ def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: ) return agent_config + def initialize_clutter_object_set(self) -> None: + """ + Get the template handles for configured clutter objects. + """ + + self.clutter_object_handles = [] + for obj_name in self.clutter_object_set: + matching_handles = ( + self.sim.metadata_mediator.object_template_manager.get_template_handles( + obj_name + ) + ) + assert ( + len(matching_handles) > 0 + ), f"No matching template for '{obj_name}' in the dataset." + self.clutter_object_handles.append(matching_handles[0]) + def reconfigure_sim( self, mm: Optional[habitat_sim.metadata.MetadataMediator] = None ) -> None: @@ -994,6 +1011,8 @@ def reconfigure_sim( self.ao_link_map = sutils.get_ao_link_id_map(self.sim) + self.dbv = DebugVisualizer(self.sim) + Timer.start() self.step = -1 @@ -1085,6 +1104,130 @@ def clear_furniture_joint_states(self): j_vel = ao.joint_velocities ao.joint_velocities = [0.0 for _ in range(len(j_vel))] + def check_rec_accessibility( + self, rec: hab_receptacle.Receptacle, max_height: float = 1.2, clean_up=True + ) -> Tuple[bool, str]: + """ + Use unoccluded navmesh snap to check whether a Receptacle is accessible. + """ + print(f"Checking Receptacle accessibility for {rec.unique_name}") + + # first check if the receptacle is close enough to the navmesh + rec_global_keypoints = sutils.get_global_keypoints_from_bb( + rec.bounds, rec.get_global_transform(self.sim) + ) + floor_point = None + for keypoint in rec_global_keypoints: + floor_point = self.sim.pathfinder.snap_point( + keypoint, island_index=self.largest_island_ix + ) + if not np.isnan(floor_point[0]): + break + if np.isnan(floor_point[0]): + print(" - Receptacle too far from active navmesh boundary.") + return False, "access_filtered" + + # then check that the height is acceptable + rec_min = min(rec_global_keypoints, key=lambda x: x[1]) + if rec_min[1] - floor_point[1] > max_height: + print( + f" - Receptacle exceeds maximum height {rec_min[1]-floor_point[1]} vs {max_height}." + ) + return False, "height_filtered" + + # try to sample 10 objects on the receptacle + target_number = 10 + obj_samp = ObjectSampler( + self.clutter_object_handles, + ["rec set"], + orientation_sample="up", + num_objects=(1, target_number), + ) + obj_samp.max_sample_attempts = len(self.clutter_object_handles) + obj_samp.max_placement_attempts = 10 + obj_samp.target_objects_number = target_number + rec_set_unique_names = [rec.unique_name] + rec_set_obj = hab_receptacle.ReceptacleSet( + "rec set", [""], [], rec_set_unique_names, [] + ) + recep_tracker = hab_receptacle.ReceptacleTracker( + {}, + {"rec set": rec_set_obj}, + ) + new_objs = obj_samp.sample(self.sim, recep_tracker, [], snap_down=True) + + # if we can't sample objects, this receptacle is out + if len(new_objs) == 0: + print(" - failed to sample any objects.") + return False, "access_filtered" + print(f" - sampled {len(new_objs)} / {target_number} objects.") + + for obj, _rec in new_objs: + self.clutter_object_instances.append(obj) + self.clutter_object_initial_states.append((obj.translation, obj.rotation)) + + # now try unoccluded navmesh snapping to the objects to test accessibility + obj_positions = [obj.translation for obj, _ in new_objs] + for obj, _ in new_objs: + obj.translation += mn.Vector3(100, 0, 0) + failure_count = 0 + + for o_ix, (obj, _) in enumerate(new_objs): + obj.translation = obj_positions[o_ix] + snap_point = unoccluded_navmesh_snap( + obj.translation, + 1.3, + self.sim.pathfinder, + self.sim, + obj.object_id, + self.largest_island_ix, + ) + # self.dbv.look_at(look_at=obj.translation, look_from=snap_point) + # self.dbv.get_observation().show() + if snap_point is None: + failure_count += 1 + obj.translation += mn.Vector3(100, 0, 0) + for o_ix, (obj, _) in enumerate(new_objs): + obj.translation = obj_positions[o_ix] + failure_rate = (float(failure_count) / len(new_objs)) * 100 + print(f" - failure_rate = {failure_rate}") + print( + f" - accessibility rate = {len(new_objs)-failure_count}|{len(new_objs)} ({100-failure_rate}%)" + ) + + accessible = failure_rate < 20 # 80% accessibility required + + if clean_up: + # removing all clutter objects currently + rom = self.sim.get_rigid_object_manager() + print(f"Removing {len(self.clutter_object_instances)} clutter objects.") + for obj in self.clutter_object_instances: + rom.remove_object_by_handle(obj.handle) + self.clutter_object_initial_states.clear() + self.clutter_object_instances.clear() + + if not accessible: + return False, "access_filtered" + + return True, "active" + + def set_filter_status_for_rec( + self, rec: hab_receptacle.Receptacle, filter_status: str + ) -> None: + filter_types = [ + "access_filtered", + "stability_filtered", + "height_filtered", + "manually_filtered", + "active", + ] + assert filter_status in filter_types + filtered_rec_name = rec.unique_name + for filter_type in filter_types: + if filtered_rec_name in self.rec_filter_data[filter_type]: + self.rec_filter_data[filter_type].remove(filtered_rec_name) + self.rec_filter_data[filter_status].append(filtered_rec_name) + def key_press_event(self, event: Application.KeyEvent) -> None: """ Handles `Application.KeyEvent` on a key press by performing the corresponding functions. @@ -1183,15 +1326,11 @@ def key_press_event(self, event: Application.KeyEvent) -> None: logger.info("Command: resample agent state from navmesh") if self.sim.pathfinder.is_loaded: new_agent_state = habitat_sim.AgentState() - largest_island_ix = get_largest_island_index( - pathfinder=self.sim.pathfinder, - sim=self.sim, - allow_outdoor=False, - ) - print(f"Largest indoor island index = {largest_island_ix}") + + print(f"Largest indoor island index = {self.largest_island_ix}") new_agent_state.position = ( self.sim.pathfinder.get_random_navigable_point( - island_index=largest_island_ix + island_index=self.largest_island_ix ) ) new_agent_state.rotation = quat_from_angle_axis( @@ -1266,7 +1405,31 @@ def key_press_event(self, event: Application.KeyEvent) -> None: self.cycleScene(False, shift_pressed=shift_pressed) elif key == pressed.T: - self.modify_param_from_term() + if shift_pressed: + # open all the AO default links + all_objects = sutils.get_all_objects(self.sim) + aos = [ + obj + for obj in all_objects + if isinstance(obj, habitat_sim.physics.ManagedArticulatedObject) + ] + for ao in aos: + default_link = sutils.get_ao_default_link(ao, True) + sutils.open_link(ao, default_link) + # compute and set the receptacle filters + for rix, rec in enumerate(self.receptacles): + rec_accessible, filter_type = self.check_rec_accessibility(rec) + self.set_filter_status_for_rec(rec, filter_type) + print(f"-- progress = {rix}/{len(self.receptacles)} --") + else: + if self.selected_rec is not None: + rec_accessible, filter_type = self.check_rec_accessibility( + self.selected_rec, clean_up=False + ) + self.set_filter_status_for_rec(self.selected_rec, filter_type) + else: + print("No selected receptacle, can't test accessibility.") + # self.modify_param_from_term() # load URDF # fixed_base = alt_pressed @@ -1340,16 +1503,6 @@ def key_press_event(self, event: Application.KeyEvent) -> None: if self.selected_object.handle == rec.parent_object_handle ] if rec_set is not None: - if len(self.clutter_object_handles) == 0: - for obj_name in self.clutter_object_set: - matching_handles = self.sim.metadata_mediator.object_template_manager.get_template_handles( - obj_name - ) - assert ( - len(matching_handles) > 0 - ), f"No matching template for '{obj_name}' in the dataset." - self.clutter_object_handles.append(matching_handles[0]) - rec_set_unique_names = [rec.unique_name for rec in rec_set] obj_samp = ObjectSampler( self.clutter_object_handles, @@ -1360,9 +1513,8 @@ def key_press_event(self, event: Application.KeyEvent) -> None: rec_set_obj = hab_receptacle.ReceptacleSet( "rec set", [""], [], rec_set_unique_names, [] ) - obj_count_dict = {rec.unique_name: 200 for rec in rec_set} recep_tracker = hab_receptacle.ReceptacleTracker( - obj_count_dict, + {}, {"rec set": rec_set_obj}, ) new_objs = obj_samp.sample( @@ -1565,7 +1717,7 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: self.mouse_cast_results.hits[0].point ) if filtered_rec is not None: - filtered_rec_name = self.get_rec_instance_name(filtered_rec) + filtered_rec_name = filtered_rec.unique_name print(f"Modified Receptacle Filter State: {filtered_rec_name}") if ( filtered_rec_name @@ -1605,6 +1757,16 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: self.rec_filter_data["manually_filtered"].append( filtered_rec_name ) + elif isinstance(obj, habitat_sim.physics.ManagedArticulatedObject): + # get the default link + default_link = sutils.get_ao_default_link(obj, True) + if default_link is None: + print("Selected AO has no default link.") + else: + if sutils.link_is_open(obj, default_link, 0.05): + sutils.close_link(obj, default_link) + else: + sutils.open_link(obj, default_link) self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() @@ -1723,6 +1885,11 @@ def navmesh_config_and_recompute(self) -> None: self.sim.pathfinder, self.navmesh_settings, ) + self.largest_island_ix = get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) def exit_event(self, event: Application.ExitEvent): """ diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py index a0d52e8b01..4fca8b30fc 100644 --- a/tools/check_siro_scenes.py +++ b/tools/check_siro_scenes.py @@ -1,6 +1,9 @@ +import json import os from collections import defaultdict -from typing import Any, Dict, List +from typing import Any, Dict, List, Optional, Tuple + +import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle # NOTE: (requires habitat-lab) get metadata for semantics import habitat.sims.habitat_simulator.sim_utilities as sutils @@ -12,10 +15,16 @@ MetadataInterface, default_metadata_dict, ) +from habitat.datasets.rearrange.navmesh_utils import ( + get_largest_island_index, + unoccluded_navmesh_snap, +) +from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer -from habitat_sim import Simulator +from habitat_sim import NavMeshSettings, Simulator from habitat_sim.metadata import MetadataMediator +from habitat_sim.physics import ManagedArticulatedObject from habitat_sim.utils.settings import default_sim_settings, make_cfg rand_colors = [mn.Color4(mn.Vector3(np.random.random(3))) for _ in range(100)] @@ -195,6 +204,288 @@ def save_region_counts_csv(region_counts: Dict[str, int], filepath: str) -> None print(f"Wrote region counts csv to {filepath}") +def check_rec_accessibility( + sim, + rec: hab_receptacle.Receptacle, + clutter_object_handles: List[str], + max_height: float = 1.2, + clean_up=True, + island_index: int = -1, +) -> Tuple[bool, str]: + """ + Use unoccluded navmesh snap to check whether a Receptacle is accessible. + """ + + assert len(clutter_object_handles) > 0 + + print(f"Checking Receptacle accessibility for {rec.unique_name}") + + # first check if the receptacle is close enough to the navmesh + rec_global_keypoints = sutils.get_global_keypoints_from_bb( + rec.bounds, rec.get_global_transform(sim) + ) + floor_point = None + for keypoint in rec_global_keypoints: + floor_point = sim.pathfinder.snap_point(keypoint, island_index=island_index) + if not np.isnan(floor_point[0]): + break + if np.isnan(floor_point[0]): + print(" - Receptacle too far from active navmesh boundary.") + return False, "access_filtered" + + # then check that the height is acceptable + rec_min = min(rec_global_keypoints, key=lambda x: x[1]) + if rec_min[1] - floor_point[1] > max_height: + print( + f" - Receptacle exceeds maximum height {rec_min[1]-floor_point[1]} vs {max_height}." + ) + return False, "height_filtered" + + # try to sample 10 objects on the receptacle + target_number = 10 + obj_samp = ObjectSampler( + clutter_object_handles, + ["rec set"], + orientation_sample="up", + num_objects=(1, target_number), + ) + obj_samp.max_sample_attempts = len(clutter_object_handles) + obj_samp.max_placement_attempts = 10 + obj_samp.target_objects_number = target_number + rec_set_unique_names = [rec.unique_name] + rec_set_obj = hab_receptacle.ReceptacleSet( + "rec set", [""], [], rec_set_unique_names, [] + ) + recep_tracker = hab_receptacle.ReceptacleTracker( + {}, + {"rec set": rec_set_obj}, + ) + + new_objs = [] + try: + new_objs = obj_samp.sample(sim, recep_tracker, [], snap_down=True) + except Exception as e: + print(f" - generation failed with internal exception {repr(e)}") + + # if we can't sample objects, this receptacle is out + if len(new_objs) == 0: + print(" - failed to sample any objects.") + return False, "access_filtered" + print(f" - sampled {len(new_objs)} / {target_number} objects.") + + # now try unoccluded navmesh snapping to the objects to test accessibility + obj_positions = [obj.translation for obj, _ in new_objs] + for obj, _ in new_objs: + obj.translation += mn.Vector3(100, 0, 0) + failure_count = 0 + + for o_ix, (obj, _) in enumerate(new_objs): + obj.translation = obj_positions[o_ix] + snap_point = unoccluded_navmesh_snap( + obj.translation, 1.3, sim.pathfinder, sim, obj.object_id, island_index + ) + # self.dbv.look_at(look_at=obj.translation, look_from=snap_point) + # self.dbv.get_observation().show() + if snap_point is None: + failure_count += 1 + obj.translation += mn.Vector3(100, 0, 0) + for o_ix, (obj, _) in enumerate(new_objs): + obj.translation = obj_positions[o_ix] + failure_rate = (float(failure_count) / len(new_objs)) * 100 + print(f" - failure_rate = {failure_rate}") + print( + f" - accessibility rate = {len(new_objs)-failure_count}|{len(new_objs)} ({100-failure_rate}%)" + ) + + accessible = failure_rate < 20 # 80% accessibility required + + if clean_up: + # removing all clutter objects currently + rom = sim.get_rigid_object_manager() + for obj, _ in new_objs: + rom.remove_object_by_handle(obj.handle) + + if not accessible: + return False, "access_filtered" + + return True, "active" + + +def init_rec_filter_data_dict() -> Dict[str, Any]: + """ + Get an empty rec_filter_data dictionary. + """ + return { + "active": [], + "manually_filtered": [], + "access_filtered": [], + "access_threshold": -1, # set in filter procedure + "stability_filtered": [], + "stability threshold": -1, # set in filter procedure + "height_filtered": [], + "max_height": 1.2, + "min_height": 0, + } + + +def write_rec_filter_json(filepath: str, json_dict: Dict[str, Any]) -> None: + """ + Write the receptacle filter json dict. + """ + + assert filepath.endswith(".json") + os.makedirs(os.path.dirname(filepath), exist_ok=True) + with open(filepath, "w") as f: + f.write(json.dumps(json_dict, indent=2)) + + +def set_filter_status_for_rec( + rec: hab_receptacle.Receptacle, + filter_status: str, + rec_filter_data: Dict[str, Any], + ignore_existing_status: Optional[List[str]] = None, +) -> None: + """ + Set the filter status of a Receptacle in the filter dictionary. + + :param rec: The Receptacle instance. + :param filter_status: The status to assign. + :param rec_filter_data: The current filter dictionary to modify. + :param ignore_existing_status: An optional list of filter types to lock, preventing re-assignment. + """ + + if ignore_existing_status is None: + ignore_existing_status = [] + filter_types = [ + "access_filtered", + "stability_filtered", + "height_filtered", + "manually_filtered", + "active", + ] + assert filter_status in filter_types + filtered_rec_name = rec.unique_name + for filter_type in filter_types: + if filtered_rec_name in rec_filter_data[filter_type]: + if filter_type in ignore_existing_status: + print( + f"Trying to assign filter status {filter_status} but existing status {filter_type} in ignore list. Aborting assignment." + ) + return + else: + rec_filter_data[filter_type].remove(filtered_rec_name) + rec_filter_data[filter_status].append(filtered_rec_name) + + +def navmesh_config_and_recompute(sim) -> None: + """ + Re-compute the navmesh with specific settings. + """ + + navmesh_settings = NavMeshSettings() + navmesh_settings.set_defaults() + navmesh_settings.agent_height = 1.3 # spot + navmesh_settings.agent_radius = 0.3 # human || spot + navmesh_settings.include_static_objects = True + sim.recompute_navmesh( + sim.pathfinder, + navmesh_settings, + ) + + +def initialize_clutter_object_set(sim) -> None: + """ + Get the template handles for configured clutter objects. + """ + clutter_object_set = [ + "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", + ] + clutter_object_handles = [] + for obj_name in clutter_object_set: + matching_handles = ( + sim.metadata_mediator.object_template_manager.get_template_handles(obj_name) + ) + assert ( + len(matching_handles) > 0 + ), f"No matching template for '{obj_name}' in the dataset." + clutter_object_handles.append(matching_handles[0]) + return clutter_object_handles + + +def run_rec_filter_analysis( + sim, out_dir: str, open_default_links: bool = True, keep_manual_filters: bool = True +) -> None: + """ + Collect all receptacles for the scene and run an accessibility check, saving the resulting filter file. + """ + + rec_filter_dict = init_rec_filter_data_dict() + + # load the clutter objects + sim.metadata_mediator.object_template_manager.load_configs( + "data/objects/ycb/configs/" + ) + clutter_object_handles = initialize_clutter_object_set(sim) + + # recompute the navmesh with expect parameters + navmesh_config_and_recompute(sim) + + # get the largest indoor island + largest_island = get_largest_island_index(sim.pathfinder, sim, allow_outdoor=False) + + # open default links + if open_default_links: + all_objects = sutils.get_all_objects(sim) + aos = [obj for obj in all_objects if isinstance(obj, ManagedArticulatedObject)] + for ao in aos: + default_link = sutils.get_ao_default_link(ao, True) + if default_link is not None: + sutils.open_link(ao, default_link) + + # keep manually filtered receptacles + ignore_existing_status = [] + if keep_manual_filters: + existing_scene_filter_file = hab_receptacle.get_scene_rec_filter_filepath( + sim.metadata_mediator, sim.curr_scene_name + ) + if existing_scene_filter_file is not None: + filter_strings = hab_receptacle.get_excluded_recs_from_filter_file( + existing_scene_filter_file, filter_types=["manually_filtered"] + ) + rec_filter_dict["manually_filtered"] = filter_strings + ignore_existing_status.append("manually_filtered") + + recs = hab_receptacle.find_receptacles( + sim, exclude_filter_strings=rec_filter_dict["manually_filtered"] + ) + + # compute and set the receptacle filters + for rix, rec in enumerate(recs): + rec_accessible, filter_type = check_rec_accessibility( + sim, rec, clutter_object_handles, island_index=largest_island + ) + set_filter_status_for_rec( + rec, + filter_type, + rec_filter_dict, + ignore_existing_status=ignore_existing_status, + ) + print(f"-- progress = {rix}/{len(recs)} --") + + filter_filepath = os.path.join( + out_dir, f"scene_filter_files/{sim.curr_scene_name}.rec_filter.json" + ) + write_rec_filter_json(filter_filepath, rec_filter_dict) + + if __name__ == "__main__": import argparse @@ -219,6 +510,12 @@ def save_region_counts_csv(region_counts: Dict[str, int], filepath: str) -> None action="store_true", help="save images during tests into the output directory.", ) + parser.add_argument( + "--recompute-filter-files", + default=False, + action="store_true", + help="Run a full accessibility check on all receptacles in the scene and save a set of filter files.", + ) args = parser.parse_args() os.makedirs(args.out_dir, exist_ok=True) @@ -266,6 +563,14 @@ def save_region_counts_csv(region_counts: Dict[str, int], filepath: str) -> None scene_out_dir = os.path.join(args.out_dir, f"{sim.curr_scene_name}/") + ########################################## + # receptacle filter computation + if args.recompute_filter_files: + run_rec_filter_analysis( + sim, args.out_dir, open_default_links=True, keep_manual_filters=True + ) + continue + ########################################## # Check region counts print(" - region counts") @@ -327,6 +632,9 @@ def save_region_counts_csv(region_counts: Dict[str, int], filepath: str) -> None missing_semantics_output, f"{obj.handle}__" ) + # short circuit other functionality for now + if args.recompute_filter_files: + exit() csv_filepath = os.path.join(args.out_dir, "siro_scene_test_results.csv") export_results_csv(csv_filepath, scene_test_results) region_count_csv_filepath = os.path.join(args.out_dir, "region_counts.csv") From d56c374b771748afdc37bb01aef0ec9f4ca4ed36 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 22 Apr 2024 09:58:52 -0700 Subject: [PATCH 150/153] add utility for "within_set" computation to the siro scene checker --- tools/check_siro_scenes.py | 61 +++++++++++++++++++++++++++++++------- 1 file changed, 51 insertions(+), 10 deletions(-) diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py index 4fca8b30fc..d103c31955 100644 --- a/tools/check_siro_scenes.py +++ b/tools/check_siro_scenes.py @@ -425,6 +425,10 @@ def run_rec_filter_analysis( ) -> None: """ Collect all receptacles for the scene and run an accessibility check, saving the resulting filter file. + + :param out_dir: Where to write the filter files. + :param open_default_links: Whether or not to open default links when considering final accessible Receptacles set. + :param keep_manual_filters: Whether to keep or override existing manual filter definitions. """ rec_filter_dict = init_rec_filter_data_dict() @@ -441,15 +445,6 @@ def run_rec_filter_analysis( # get the largest indoor island largest_island = get_largest_island_index(sim.pathfinder, sim, allow_outdoor=False) - # open default links - if open_default_links: - all_objects = sutils.get_all_objects(sim) - aos = [obj for obj in all_objects if isinstance(obj, ManagedArticulatedObject)] - for ao in aos: - default_link = sutils.get_ao_default_link(ao, True) - if default_link is not None: - sutils.open_link(ao, default_link) - # keep manually filtered receptacles ignore_existing_status = [] if keep_manual_filters: @@ -467,11 +462,21 @@ def run_rec_filter_analysis( sim, exclude_filter_strings=rec_filter_dict["manually_filtered"] ) - # compute and set the receptacle filters + # compute a map from parent object to Receptacles + parent_handle_to_rec: Dict[str, List[hab_receptacle.Receptacle]] = defaultdict( + lambda: [] + ) + for rec in recs: + parent_handle_to_rec[rec.parent_object_handle].append(rec) + + # compute the default accessibility with all closed links + default_active_set: List[hab_receptacle.Receptacle] = [] for rix, rec in enumerate(recs): rec_accessible, filter_type = check_rec_accessibility( sim, rec, clutter_object_handles, island_index=largest_island ) + if rec_accessible: + default_active_set.append(rec) set_filter_status_for_rec( rec, filter_type, @@ -480,6 +485,42 @@ def run_rec_filter_analysis( ) print(f"-- progress = {rix}/{len(recs)} --") + # open default links and re-compute accessibility for each AO + # the difference between default state accessibility and open state accessibility defines the "within_set" + within_set: List[hab_receptacle.Receptacle] = [] + if open_default_links: + all_objects = sutils.get_all_objects(sim) + aos = [obj for obj in all_objects if isinstance(obj, ManagedArticulatedObject)] + for aoix, ao in enumerate(aos): + default_link = sutils.get_ao_default_link(ao, True) + if default_link is not None: + sutils.open_link(ao, default_link) + # recompute accessibility + for child_rec in parent_handle_to_rec[ao.handle]: + rec_accessible, filter_type = check_rec_accessibility( + sim, + child_rec, + clutter_object_handles, + island_index=largest_island, + ) + if rec_accessible and child_rec not in default_active_set: + # found a Receptacle which is only accessible when the default_link is open + within_set.append(child_rec) + set_filter_status_for_rec( + child_rec, + filter_type, + rec_filter_dict, + ignore_existing_status=ignore_existing_status, + ) + sutils.close_link(ao, default_link) + print(f"-- progress = {aoix}/{len(aos)} --") + + # write the within set to the filter file + rec_filter_dict["within_set"] = [ + within_rec.unique_name for within_rec in within_set + ] + + # write the filter file to JSON filter_filepath = os.path.join( out_dir, f"scene_filter_files/{sim.curr_scene_name}.rec_filter.json" ) From 33e33024edfb550628c28dd52cf7edc6f83c7856 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 26 Apr 2024 09:09:54 -0700 Subject: [PATCH 151/153] refactor to allow non TriangleMesh receptacles and AnyObjectReceptacles --- examples/viewer.py | 76 ++++++++++++++++++++++++++++++++++++---------- 1 file changed, 60 insertions(+), 16 deletions(-) diff --git a/examples/viewer.py b/examples/viewer.py index 9738447e47..a2afca9349 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -262,7 +262,8 @@ def __init__( self.rec_access_filter_threshold = 0.12 # empirically chosen self.rec_color_mode = RecColorMode.FILTERING # map receptacle to parent objects - self.rec_to_poh: Dict[hab_receptacle.Receptacle, str] = {} + self.rec_to_poth: Dict[hab_receptacle.Receptacle, str] = {} + self.poh_to_rec: Dict[str, List[hab_receptacle.Receptacle]] = {} # contains filtering metadata and classification of meshes filtered automatically and manually self.rec_filter_data = None # TODO need to determine filter path for each scene during tabbing? @@ -390,7 +391,7 @@ def load_scene_filter_file(self): f"WARNING: No rec filter file configured for scene {self.sim.curr_scene_name}." ) - def get_closest_tri_receptacle( + def get_closest_receptacle( self, pos: mn.Vector3, max_dist: float = 3.5 ) -> Optional[hab_receptacle.TriangleMeshReceptacle]: """ @@ -405,19 +406,45 @@ def get_closest_tri_receptacle( return None closest_rec = None closest_rec_dist = max_dist - for receptacle in self.receptacles: + recs = ( + self.receptacles + if ( + self.selected_object is None + or self.selected_object.handle not in self.poh_to_rec + ) + else self.poh_to_rec[self.selected_object.handle] + ) + for receptacle in recs: g_trans = receptacle.get_global_transform(self.sim) # transform the query point once instead of all verts local_point = g_trans.inverted().transform_point(pos) if (g_trans.translation - pos).length() < max_dist: # receptacles object transform should be close to the point - for vert in receptacle.mesh_data.attribute( - mn.trade.MeshAttribute.POSITION - ): - v_dist = (local_point - vert).length() - if v_dist < closest_rec_dist: - closest_rec_dist = v_dist - closest_rec = receptacle + if isinstance(receptacle, hab_receptacle.TriangleMeshReceptacle): + for vert in receptacle.mesh_data.attribute( + mn.trade.MeshAttribute.POSITION + ): + v_dist = (local_point - vert).length() + if v_dist < closest_rec_dist: + closest_rec_dist = v_dist + closest_rec = receptacle + else: + global_keypoints = None + if isinstance(receptacle, hab_receptacle.AABBReceptacle): + global_keypoints = sutils.get_global_keypoints_from_bb( + receptacle.bounds, g_trans + ) + elif isinstance(receptacle, hab_receptacle.AnyObjectReceptacle): + global_keypoints = sutils.get_bb_corners( + receptacle._get_global_bb(self.sim) + ) + + for g_point in global_keypoints: + v_dist = (pos - g_point).length() + if v_dist < closest_rec_dist: + closest_rec_dist = v_dist + closest_rec = receptacle + return closest_rec def compute_rec_filter_state( @@ -459,7 +486,7 @@ def compute_rec_filter_state( rec_unique_name = rec.unique_name # respect already marked receptacles if rec_unique_name not in self.rec_filter_data["manually_filtered"]: - rec_dat = self._cpo.gt_data[self.rec_to_poh[rec]]["receptacles"][ + rec_dat = self._cpo.gt_data[self.rec_to_poth[rec]]["receptacles"][ rec.name ] rec_shape_data = rec_dat["shape_id_results"][filter_shape] @@ -531,11 +558,14 @@ def load_receptacles(self): if "collision_stand-in" not in rec.parent_object_handle ] for receptacle in self.receptacles: - if receptacle not in self.rec_to_poh: + if receptacle not in self.rec_to_poth: po_handle = sutils.get_obj_from_handle( self.sim, receptacle.parent_object_handle ).creation_attributes.handle - self.rec_to_poh[receptacle] = po_handle + self.rec_to_poth[receptacle] = po_handle + if receptacle.parent_object_handle not in self.poh_to_rec: + self.poh_to_rec[receptacle.parent_object_handle] = [] + self.poh_to_rec[receptacle.parent_object_handle].append(receptacle) def add_col_proxy_object( self, obj_instance: habitat_sim.physics.ManagedRigidObject @@ -667,7 +697,7 @@ def debug_draw(self): rec_dat = None if self.cpo_initialized: - rec_dat = self._cpo.gt_data[self.rec_to_poh[receptacle]][ + rec_dat = self._cpo.gt_data[self.rec_to_poth[receptacle]][ "receptacles" ][receptacle.name] @@ -1510,6 +1540,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None: orientation_sample="up", num_objects=(1, 10), ) + obj_samp.receptacle_instances = self.receptacles rec_set_obj = hab_receptacle.ReceptacleSet( "rec set", [""], [], rec_set_unique_names, [] ) @@ -1700,6 +1731,10 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: print("This is the stage.") else: obj = sutils.get_obj_from_id(self.sim, hit_id) + link_id = None + if obj.object_id != hit_id: + # this is a link + link_id = obj.link_object_ids[hit_id] self.selected_object = obj print(f"Object: {obj.handle}") if self.receptacles is not None: @@ -1707,13 +1742,22 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: if rec.parent_object_handle == obj.handle: print(f" - Receptacle: {rec.name}") if shift_pressed: - self.selected_rec = self.get_closest_tri_receptacle( + if obj.handle not in self.poh_to_rec: + new_rec = hab_receptacle.AnyObjectReceptacle( + obj.handle + "_aor", + parent_object_handle=obj.handle, + parent_link=link_id, + ) + self.receptacles.append(new_rec) + self.poh_to_rec[obj.handle] = [new_rec] + self.rec_to_poth[new_rec] = obj.creation_attributes.handle + self.selected_rec = self.get_closest_receptacle( self.mouse_cast_results.hits[0].point ) if self.selected_rec is not None: print(f"Selected Receptacle: {self.selected_rec.name}") elif alt_pressed: - filtered_rec = self.get_closest_tri_receptacle( + filtered_rec = self.get_closest_receptacle( self.mouse_cast_results.hits[0].point ) if filtered_rec is not None: From e795404376e4fbb4f4d9446296630b2791fc635e Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 7 May 2024 10:08:51 -0700 Subject: [PATCH 152/153] refactor check_siro_scenes.py --- tools/check_siro_scenes.py | 153 +++++++++++++++++++++++-------------- 1 file changed, 94 insertions(+), 59 deletions(-) diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py index d103c31955..42c720b807 100644 --- a/tools/check_siro_scenes.py +++ b/tools/check_siro_scenes.py @@ -42,7 +42,7 @@ def to_str_csv(data: Any) -> str: if isinstance(data, list): list_str = "" for elem in data: - list_str += f"{elem} |" + list_str += f"{elem};" return list_str raise NotImplementedError(f"Data type {type(data)} is not supported in csv string.") @@ -552,13 +552,34 @@ def run_rec_filter_analysis( help="save images during tests into the output directory.", ) parser.add_argument( - "--recompute-filter-files", - default=False, - action="store_true", - help="Run a full accessibility check on all receptacles in the scene and save a set of filter files.", + "--actions", + nargs="+", + type=str, + help="A set of strings indicating check actions to be performed on the dataset.", + default=None, ) args = parser.parse_args() + available_check_actions = [ + "rec_unique_names", + "rec_filters", + "region_counts", + "joint_popping", + "visualize_regions", + "analyze_semantics", + ] + + target_check_actions = [] + + assert args.actions is not None, "Must select and action." + + for target_action in args.actions: + assert ( + target_action in available_check_actions + ), f"provided action {target_action} is not in the valid set: {available_check_actions}" + + target_check_actions = args.actions + os.makedirs(args.out_dir, exist_ok=True) # create an initial simulator config @@ -604,79 +625,93 @@ def run_rec_filter_analysis( scene_out_dir = os.path.join(args.out_dir, f"{sim.curr_scene_name}/") + ########################################## + # gather all Receptacle.unique_name in the scene + if "rec_unique_names" in target_check_actions: + all_recs = hab_receptacle.find_receptacles(sim) + unique_names = [rec.unique_name for rec in all_recs] + scene_test_results[sim.curr_scene_name][ + "rec_unique_names" + ] = unique_names + ########################################## # receptacle filter computation - if args.recompute_filter_files: + if "rec_filters" in target_check_actions: run_rec_filter_analysis( sim, args.out_dir, open_default_links=True, keep_manual_filters=True ) - continue ########################################## # Check region counts - print(" - region counts") - scene_region_counts = get_region_counts(sim) - for region_name, count in scene_region_counts.items(): - region_counts[region_name] += count + if "region_counts" in target_check_actions: + print(" - region counts") + scene_region_counts = get_region_counts(sim) + for region_name, count in scene_region_counts.items(): + region_counts[region_name] += count ########################################## # Check for joint popping - print(" - check joint popping") - unstable_aos, joint_errors = check_joint_popping( - sim, out_dir=scene_out_dir if args.save_images else None, dbv=dbv - ) - if len(unstable_aos) > 0: - scene_test_results[sim.curr_scene_name]["unstable_aos"] = "" - for ix, ao_handle in enumerate(unstable_aos): - scene_test_results[sim.curr_scene_name][ - "unstable_aos" - ] += f"{ao_handle}({joint_errors[ix]}) | " + if "joint_popping" in target_check_actions: + print(" - check joint popping") + unstable_aos, joint_errors = check_joint_popping( + sim, out_dir=scene_out_dir if args.save_images else None, dbv=dbv + ) + if len(unstable_aos) > 0: + scene_test_results[sim.curr_scene_name]["unstable_aos"] = "" + for ix, ao_handle in enumerate(unstable_aos): + scene_test_results[sim.curr_scene_name][ + "unstable_aos" + ] += f"{ao_handle}({joint_errors[ix]}) | " ############################################ # analyze and visualize regions - print(" - check and visualize regions") - if args.save_images: - save_region_visualizations( - sim, os.path.join(scene_out_dir, "regions/"), dbv - ) - expected_regions = ["kitchen", "living room", "bedroom"] - all_region_cats = [ - region.category.name() for region in sim.semantic_scene.regions - ] - missing_expected_regions = [ - expected_region - for expected_region in expected_regions - if expected_region not in all_region_cats - ] - if len(missing_expected_regions) > 0: - scene_test_results[sim.curr_scene_name]["missing_expected_regions"] = "" - for expected_region in missing_expected_regions: + if "visualize_regions" in target_check_actions: + print(" - check and visualize regions") + if args.save_images: + save_region_visualizations( + sim, os.path.join(scene_out_dir, "regions/"), dbv + ) + expected_regions = ["kitchen", "living room", "bedroom"] + all_region_cats = [ + region.category.name() for region in sim.semantic_scene.regions + ] + missing_expected_regions = [ + expected_region + for expected_region in expected_regions + if expected_region not in all_region_cats + ] + if len(missing_expected_regions) > 0: scene_test_results[sim.curr_scene_name][ "missing_expected_regions" - ] += f"{expected_region} | " + ] = "" + for expected_region in missing_expected_regions: + scene_test_results[sim.curr_scene_name][ + "missing_expected_regions" + ] += f"{expected_region} | " ############################################## # analyze semantics - print(" - check and visualize semantics") - scene_test_results[sim.curr_scene_name][ - "objects_missing_semantic_class" - ] = [] - missing_semantics_output = os.path.join(scene_out_dir, "missing_semantics/") - for obj in sutils.get_all_objects(sim): - if mi.get_object_instance_category(obj) is None: - scene_test_results[sim.curr_scene_name][ - "objects_missing_semantic_class" - ].append(obj.handle) - if args.save_images: - os.makedirs(missing_semantics_output, exist_ok=True) - dbv.peek(obj, peek_all_axis=True).save( - missing_semantics_output, f"{obj.handle}__" - ) + if "analyze_semantics" in target_check_actions: + print(" - check and visualize semantics") + scene_test_results[sim.curr_scene_name][ + "objects_missing_semantic_class" + ] = [] + missing_semantics_output = os.path.join( + scene_out_dir, "missing_semantics/" + ) + for obj in sutils.get_all_objects(sim): + if mi.get_object_instance_category(obj) is None: + scene_test_results[sim.curr_scene_name][ + "objects_missing_semantic_class" + ].append(obj.handle) + if args.save_images: + os.makedirs(missing_semantics_output, exist_ok=True) + dbv.peek(obj, peek_all_axis=True).save( + missing_semantics_output, f"{obj.handle}__" + ) - # short circuit other functionality for now - if args.recompute_filter_files: - exit() csv_filepath = os.path.join(args.out_dir, "siro_scene_test_results.csv") export_results_csv(csv_filepath, scene_test_results) - region_count_csv_filepath = os.path.join(args.out_dir, "region_counts.csv") - save_region_counts_csv(region_counts, region_count_csv_filepath) + if "region_counts" in target_check_actions: + region_count_csv_filepath = os.path.join(args.out_dir, "region_counts.csv") + save_region_counts_csv(region_counts, region_count_csv_filepath) From 799a61a0f116154cfa2fec33699087b118343ac9 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Tue, 7 May 2024 12:20:50 -0700 Subject: [PATCH 153/153] add faucet point visualizer to the check set --- tools/check_siro_scenes.py | 52 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 50 insertions(+), 2 deletions(-) diff --git a/tools/check_siro_scenes.py b/tools/check_siro_scenes.py index 42c720b807..050cc30a92 100644 --- a/tools/check_siro_scenes.py +++ b/tools/check_siro_scenes.py @@ -4,8 +4,6 @@ from typing import Any, Dict, List, Optional, Tuple import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle - -# NOTE: (requires habitat-lab) get metadata for semantics import habitat.sims.habitat_simulator.sim_utilities as sutils import magnum as mn import numpy as np @@ -22,6 +20,8 @@ from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler from habitat.sims.habitat_simulator.debug_visualizer import DebugVisualizer +# NOTE: (requires habitat-lab) get metadata for semantics +import habitat_sim from habitat_sim import NavMeshSettings, Simulator from habitat_sim.metadata import MetadataMediator from habitat_sim.physics import ManagedArticulatedObject @@ -527,6 +527,40 @@ def run_rec_filter_analysis( write_rec_filter_json(filter_filepath, rec_filter_dict) +def get_global_faucet_points(sim: habitat_sim.Simulator) -> Dict[str, List[mn.Vector3]]: + """ + Gets a global set of points identifying faucets for each object in the scene. + Returns a dict mapping object handles to global points. + """ + objs = sutils.get_all_objects(sim) + obj_markersets = {} + for obj in objs: + all_obj_marker_sets = obj.marker_sets + if all_obj_marker_sets.has_taskset("faucets"): + # this object has faucet annotations + obj_markersets[obj.handle] = [] + faucet_marker_sets = all_obj_marker_sets.get_taskset_points("faucets") + for link_name, link_faucet_markers in faucet_marker_sets.items(): + link_id = -1 + if link_name != "root": + link_id = obj.get_link_id_from_name(link_name) + for _marker_subset_name, points in link_faucet_markers.items(): + global_points = obj.transform_local_pts_to_world(points, link_id) + obj_markersets[obj.handle].extend(global_points) + return obj_markersets + + +def draw_global_point_set(global_points: List[mn.Vector3], debug_line_render): + for point in global_points: + debug_line_render.draw_circle( + translation=point, + radius=0.02, + normal=mn.Vector3(0, 1, 0), + color=mn.Color4.red(), + num_segments=12, + ) + + if __name__ == "__main__": import argparse @@ -561,6 +595,7 @@ def run_rec_filter_analysis( args = parser.parse_args() available_check_actions = [ + "faucet_points", "rec_unique_names", "rec_filters", "region_counts", @@ -625,6 +660,19 @@ def run_rec_filter_analysis( scene_out_dir = os.path.join(args.out_dir, f"{sim.curr_scene_name}/") + ########################################## + # get images of all the global faucet points in the scene + if "faucet_points" in target_check_actions: + scene_obj_global_faucet_points = get_global_faucet_points(sim) + for obj_handle, global_points in scene_obj_global_faucet_points.items(): + circles = [ + (point, 0.1, mn.Vector3(0, 1, 0), mn.Color4.red()) + for point in global_points + ] + dbv.peek( + obj_handle, peek_all_axis=True, debug_circles=circles + ).save(scene_out_dir, f"faucets_{obj_handle}_") + ########################################## # gather all Receptacle.unique_name in the scene if "rec_unique_names" in target_check_actions: