diff --git a/docs/conf.py b/docs/conf.py index 981d0a51b0..2d102f6de9 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -21,8 +21,23 @@ # Monkey patch the registry to be the _Registry class instead of the singleton for docs habitat_sim.registry = type(habitat_sim.registry) # TODO: remove once utils/__init__.py is removed again -habitat_sim.utils.__all__.remove("quat_from_angle_axis") -habitat_sim.utils.__all__.remove("quat_rotate_vector") +habitat_sim.utils.common.__all__ = [ + x + for x in habitat_sim.utils.common.__all__ + if x + not in [ + "quat_from_coeffs", + "quat_to_coeffs", + "quat_from_magnum", + "quat_to_magnum", + "quat_from_angle_axis", + "quat_to_angle_axis", + "quat_rotate_vector", + "quat_from_two_vectors", + ] +] +# habitat_sim.utils.__all__.remove("quat_from_angle_axis") +# habitat_sim.utils.__all__.remove("quat_rotate_vector") PROJECT_TITLE = "Habitat" PROJECT_SUBTITLE = "Sim Docs" diff --git a/examples/marker_viewer.py b/examples/marker_viewer.py new file mode 100644 index 0000000000..1d5bb5911b --- /dev/null +++ b/examples/marker_viewer.py @@ -0,0 +1,1271 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import 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 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.classes import MarkerSetsEditor, ObjectEditor, SemanticDisplay +from habitat_sim.utils.common import quat_from_angle_axis +from habitat_sim.utils.namespace import hsim_physics +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +# file holding all URDF filenames +URDF_FILES = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "urdfFileNames.txt" +) +# file holding hashes of objects that have no links +NOLINK_URDF_FILES = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "urdfsWithNoLinks.txt" +) + + +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 = 512 + + # 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, + ) + # Set blend function + 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.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 + + # 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.last_hit_details = None + + self.navmesh_dirty = False + + # mouse raycast visualization + self.mouse_cast_results = None + self.mouse_cast_has_hits = False + + self.ao_link_map = None + + self.agent_start_location = mn.Vector3(-5.7, 0.0, -4.0) + self.ao_place_location = mn.Vector3(-7.7, 1.0, -4.0) + + # Load simulatioon scene + self.reconfigure_sim() + + # load file holding urdf filenames needing handles + print( + f"URDF hashes file name : {URDF_FILES} | No-link URDFS file name : {NOLINK_URDF_FILES}" + ) + # Build a List of URDF hash names loaded from disk, where each entry + # is a dictionary of hash, status, and notes (if present). + # As URDFs are completed their status is changed from "unfinished" to "done" + self.urdf_hash_names_list = self.load_urdf_filenames() + # Start with first idx in self.urdf_hash_names_list + self.urdf_edit_hash_idx = self._get_next_hash_idx( + start_idx=0, forward=True, status="unfinished" + ) + + # load markersets for every object and ao into a cache + task_names_set = {"faucets", "handles"} + self.markersets_util = MarkerSetsEditor(self.sim, task_names_set) + self.markersets_util.set_current_taskname("handles") + + # Editing for object selection + self.obj_editor = ObjectEditor(self.sim) + # Set default editing to rotation + self.obj_editor.set_edit_mode_rotate() + # Force save of urdf hash to NOLINK_URDF_FILES file + self.force_urdf_notes_save = False + + # Load first object to place markers on + self.load_urdf_obj() + + # Semantics + self.dbg_semantics = SemanticDisplay(self.sim) + + # 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 _get_next_hash_idx(self, start_idx: int, forward: bool, status: str): + if forward: + iter_range = range(start_idx, len(self.urdf_hash_names_list)) + else: + iter_range = range(start_idx, -1, -1) + + for i in iter_range: + if self.urdf_hash_names_list[i]["status"] == status: + return i + print( + f"No {status} hashes left to be found {('forward of' if forward else 'backward from')} starting idx {start_idx}." + ) + return -1 + + def _set_hash_list_status(self, hash_idx: int, is_finished: bool): + pass + + def load_urdf_filenames(self): + # list of dicts holding hash, status, notes + urdf_hash_names_list: List[Dict[str:str]] = [] + # File names of all URDFs + with open(URDF_FILES, "r") as f: + for line in f.readlines(): + vals = line.split(",", maxsplit=2) + finished = "done" if vals[1].strip().lower() == "true" else "unfinished" + new_dict = {"hash": vals[0].strip(), "status": finished} + if len(vals) > 2: + new_dict["notes"] = vals[2].strip() + else: + new_dict["notes"] = "" + urdf_hash_names_list.append(new_dict) + + return urdf_hash_names_list + + def update_nolink_file(self, save_no_markers: bool): + # remove urdf hash from NOLINK_URDF_FILES if it has links, add it if it does not + urdf_hash: str = self.urdf_hash_names_list[self.urdf_edit_hash_idx]["hash"] + # preserve all text in file after comma + urdf_nolink_hash_names: Dict[str, str] = {} + with open(NOLINK_URDF_FILES, "r") as f: + for line in f.readlines(): + if len(line.strip()) == 0: + continue + vals = line.split(",", maxsplit=1) + # hash is idx0; notes is idx1 + urdf_nolink_hash_names[vals[0]] = vals[1].strip() + if save_no_markers: + # it has no markers or we are forcing a save, so add it to record if it isn't already there + if urdf_hash not in urdf_nolink_hash_names: + # add empty string + urdf_nolink_hash_names[urdf_hash] = "" + else: + # if it has markers now, remove it from record + urdf_nolink_hash_names.pop(urdf_hash, None) + # save no-link status results + with open(NOLINK_URDF_FILES, "w") as f: + for file_hash, notes in urdf_nolink_hash_names.items(): + f.write(f"{file_hash}, {notes}\n") + + def save_urdf_filesnames(self): + # save current state of URDF files + with open(URDF_FILES, "w") as f: + for urdf_entry in self.urdf_hash_names_list: + notes = "" if len(urdf_entry) > 2 else f", {urdf_entry['notes']}" + status = urdf_entry["status"].strip().lower() == "done" + f.write(f"{urdf_entry['hash']}, {status}{notes}\n") + + def _delete_sel_obj_update_nolink_file(self, sel_obj_hash: str): + sel_obj = self.obj_editor.get_target_sel_obj() + if sel_obj is None: + sel_obj = hsim_physics.get_obj_from_handle(sel_obj_hash) + + save_no_markers = ( + self.force_urdf_notes_save + or sel_obj.marker_sets.num_tasksets == 0 + or (not sel_obj.marker_sets.has_taskset("handles")) + ) + print( + f"Object {sel_obj.handle} has {sel_obj.marker_sets.num_tasksets} tasksets and Force save set to {self.force_urdf_notes_save} == Save as no marker urdf? {save_no_markers}" + ) + # remove currently selected objects + removed_obj_handles = self.obj_editor.remove_sel_objects() + # should only have 1 handle + for handle in removed_obj_handles: + print(f"Removed {handle}") + # finalize removal + self.obj_editor.remove_all_objs() + # update record of object hashes with/without markers with current file's state + self.update_nolink_file(save_no_markers=save_no_markers) + + def load_urdf_obj(self): + # Next object to be edited + sel_obj_hash = self.urdf_hash_names_list[self.urdf_edit_hash_idx]["hash"] + print(f"URDF hash we want : `{sel_obj_hash}`") + # Load object into scene + _, self.navmesh_dirty = self.obj_editor.load_from_substring( + navmesh_dirty=self.navmesh_dirty, + obj_substring=sel_obj_hash, + build_loc=self.ao_place_location, + ) + self.ao_link_map = hsim_physics.get_ao_link_id_map(self.sim) + self.markersets_util.update_markersets() + self.markersets_util.set_current_taskname("handles") + + def cycle_through_urdfs(self, shift_pressed: bool) -> None: + # current object hash + old_sel_obj_hash = self.urdf_hash_names_list[self.urdf_edit_hash_idx]["hash"] + # Determine the status we are looking for when we search for the next desired index + if shift_pressed: + status = "done" + start_idx = self.urdf_edit_hash_idx + else: + status = "unfinished" + start_idx = self.urdf_edit_hash_idx + # Moving forward - set current to finished + self.urdf_hash_names_list[self.urdf_edit_hash_idx]["status"] = "done" + + # Get the idx of the next object we want to edit + # Either the idx of the next record that is unfinished, or the most recent previous record that is done + next_idx = self._get_next_hash_idx( + start_idx=start_idx, forward=not shift_pressed, status=status + ) + + # If we don't have a valid next index then handle edge case + if next_idx == -1: + if not shift_pressed: + # save current status + self.save_urdf_filesnames() + # moving forward - done! + print( + f"Finished going through all {len(self.urdf_hash_names_list)} loaded urdf files. Exiting." + ) + self.exit_event(Application.ExitEvent) + else: + # moving backward, at the start of all the objects so nowhere to go + print(f"No objects previous to current object {old_sel_obj_hash}.") + return + # set edited state in urdf file list appropriately if moving backward, set previous to unfinished, leave current unfinished + if shift_pressed: + # Moving backward - set previous to unfinished, leave current unchanged + self.urdf_hash_names_list[next_idx]["status"] = "unfinished" + + # remove the current selected object and update the no_link file + self._delete_sel_obj_update_nolink_file(sel_obj_hash=old_sel_obj_hash) + + # Update the current edit hash idx + self.urdf_edit_hash_idx = next_idx + # save current status + self.save_urdf_filesnames() + print(f"URDF hash we just finished : `{old_sel_obj_hash}`") + # load next urdf object + self.load_urdf_obj() + # reset force save to False for each object + self.force_urdf_notes_save = False + + 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. + 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() + 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 + 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 + 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, + ) + 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. + """ + + debug_line_render = self.sim.get_debug_line_render() + 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(debug_line_render) + # draw semantic information + self.dbg_semantics.draw_region_debug(debug_line_render=debug_line_render) + # draw markersets information + if self.markersets_util.marker_sets_per_obj is not None: + self.markersets_util.draw_marker_sets_debug( + debug_line_render, + self.render_camera.render_camera.node.absolute_translation, + ) + + self.obj_editor.draw_selected_objects(debug_line_render) + # mouse raycast circle + # This is confusing with the marker placement + # if self.mouse_cast_has_hits: + # debug_line_render.draw_circle( + # translation=self.mouse_cast_results.hits[0].point, + # radius=0.005, + # color=mn.Color4(mn.Vector3(1.0), 1.0), + # normal=self.mouse_cast_results.hits[0].normal, + # ) + + 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() + 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( + 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["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 + + 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) + + new_agent_state = habitat_sim.AgentState() + new_agent_state.position = self.agent_start_location + new_agent_state.rotation = quat_from_angle_axis( + 0.5 * np.pi, + np.array([0, 1, 0]), + ) + self.default_agent.set_state(new_agent_state) + + 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) + + self.ao_link_map = hsim_physics.get_ao_link_id_map(self.sim) + # 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." + + 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. + """ + if repetitions == 0: + return + + agent = self.sim.agents[self.agent_id] + 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] + + for _ in range(int(repetitions)): + [agent.act(x) for x in action_queue] + + 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.TAB: + self.cycle_through_urdfs(shift_pressed=shift_pressed) + + 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.B: + # Save all markersets that have been changed + self.markersets_util.save_all_dirty_markersets() + + elif key == pressed.C: + self.contact_debug_draw = not self.contact_debug_draw + log_str = f"Command: toggle contact debug draw: {self.contact_debug_draw}" + if self.contact_debug_draw: + # perform a discrete collision detection pass and enable contact debug drawing to visualize the results + # TODO: add a nice log message with concise contact pair naming. + log_str = f"{log_str}: performing discrete collision detection and visualize active contacts." + self.sim.perform_discrete_collision_detection() + logger.info(log_str) + elif key == pressed.Q: + # rotate selected object(s) to left + self.navmesh_dirty = self.obj_editor.edit_left(self.navmesh_dirty) + elif key == pressed.E: + # rotate selected object(s) right + self.navmesh_dirty = self.obj_editor.edit_right(self.navmesh_dirty) + elif key == pressed.R: + # cycle through rotation amount + self.obj_editor.change_edit_vals(toggle=shift_pressed) + elif key == pressed.F: + self.force_urdf_notes_save = not self.force_urdf_notes_save + print( + f"Force save of hash to URDF notes file set to {self.force_urdf_notes_save}" + ) + elif key == pressed.G: + # If shift pressed then open, otherwise close + # If alt pressed then selected, otherwise all + self.obj_editor.set_ao_joint_states( + do_open=shift_pressed, selected=alt_pressed + ) + if not shift_pressed: + # if closing then redo navmesh + self.navmesh_config_and_recompute() + elif key == pressed.H: + self.print_help_text() + elif key == pressed.K: + # Cyle through semantics display + info_str = self.dbg_semantics.cycle_semantic_region_draw() + logger.info(info_str) + + 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() + 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") + + elif key == pressed.V: + # self.invert_gravity() + # logger.info("Command: gravity inverted") + # Duplicate all the selected objects and place them in the scene + # or inject a new object by queried handle substring in front of + # the agent if no objects selected + + new_obj_list, self.navmesh_dirty = self.obj_editor.build_objects( + self.navmesh_dirty, + build_loc=self.ao_place_location, + ) + if len(new_obj_list) == 0: + print("Failed to add any new objects.") + else: + print(f"Finished adding {len(new_obj_list)} object(s).") + self.ao_link_map = hsim_physics.get_ao_link_id_map(self.sim) + self.markersets_util.update_markersets() + + # 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 calc_mouse_cast_results(self, screen_location: mn.Vector3) -> None: + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(screen_location)) + mouse_cast_results = self.sim.cast_ray(ray=ray) + self.mouse_cast_has_hits = ( + mouse_cast_results is not None and mouse_cast_results.has_hits() + ) + self.mouse_cast_results = mouse_cast_results + + 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. + """ + 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] + + 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 MARKER mode : + LEFT CLICK : places a marker at mouse position on targeted object if not the stage + RIGHT CLICK : removes the closest marker to mouse position on targeted object + """ + button = Application.MouseEvent.Button + physics_enabled = self.sim.get_physics_simulation_library() + mod = Application.InputEvent.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + bool(event.modifiers & mod.ALT) + self.calc_mouse_cast_results(event.position) + + if physics_enabled and self.mouse_cast_has_hits: + # If look enabled + if self.mouse_interaction == MouseMode.LOOK: + mouse_cast_results = self.mouse_cast_results + if event.button == button.RIGHT: + # Find object being clicked + obj_found = False + obj = None + # find first non-stage object + hit_idx = 0 + 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 + obj = hsim_physics.get_obj_from_id(self.sim, hit_obj_id) + if obj is None: + hit_idx += 1 + else: + obj_found = True + if obj_found: + print( + f"Object: {obj.handle} is {'Articlated' if obj.is_articulated else 'Rigid'} Object at {obj.translation}" + ) + else: + print("This is the stage.") + + if not shift_pressed: + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(obj) + elif obj_found: + # add or remove object from selected objects, depending on whether it is already selected or not + self.obj_editor.toggle_sel_obj(obj) + # else if marker enabled + elif self.mouse_interaction == MouseMode.MARKER: + # hit_info = self.mouse_cast_results.hits[0] + sel_obj = self.markersets_util.place_marker_at_hit_location( + self.mouse_cast_results.hits[0], + self.ao_link_map, + event.button == button.LEFT, + ) + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(sel_obj) + + 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 MARKER mode, wheel cycles through available taskset names + """ + 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) + bool(event.modifiers & Application.InputEvent.Modifier.ALT) + 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.MARKER: + self.markersets_util.cycle_current_taskname(scroll_mod_val > 0) + 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 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.MARKER + elif self.mouse_interaction == MouseMode.MARKER: + 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. + """ + if self.cfg.sim_cfg.scene_id.lower() == "none": + return + 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.navmesh_settings.include_static_objects = True + self.sim.recompute_navmesh( + self.sim.pathfinder, + self.navmesh_settings, + ) + + 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): + # 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] + + 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.MARKER: + mouse_mode_string = "MARKER" + edit_string = self.obj_editor.edit_disp_str() + self.window_text.render( + f""" +{self.fps} FPS +Sensor Type: {sensor_type_string} +Sensor Subtype: {sensor_subtype_string} +{edit_string} +Selected MarkerSets TaskSet name : {self.markersets_util.get_current_taskname()} +Mouse Interaction Mode: {mouse_mode_string} +FORCE SAVE URDF HASH IN NOTES FILE : {self.force_urdf_notes_save} + """ + ) + 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. + """ + 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 MARKER mode : + LEFT CLICK : Add a marker to the target object at the mouse location, if not the stage + RIGHT CLICK : Remove the closest marker to the mouse location on the target object + +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. + 'g' : Modify AO link states : + (+SHIFT) : Open Selected AO + (-SHIFT) : Close Selected AO + 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 + MARKER = 2 + + +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="default", + type=str, + metavar="DATASET", + help='dataset configuration file to use (default: "default")', + ) + parser.add_argument( + "--disable-physics", + action="store_true", + help="disable physics simulation (default: False)", + ) + parser.add_argument( + "--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="Enable horizon-based ambient occlusion, which provides soft shadows in corners and crevices.", + ) + 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["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["default_agent_navmesh"] = False + sim_settings["enable_hbao"] = args.hbao + + # start the application + HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/examples/obj_viewer.py b/examples/obj_viewer.py new file mode 100644 index 0000000000..0c241d8b41 --- /dev/null +++ b/examples/obj_viewer.py @@ -0,0 +1,1419 @@ +# 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], + 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 = ( + 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.cfg.metadata_mediator = mm + 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 + 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.target_object + )[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 + 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: + 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) + + otm = self.sim.metadata_mediator.object_template_manager + otm.load_configs("data/objects/ycb/configs/") + + 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_object2( + 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 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 + 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 = 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( + 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.navmesh_settings.include_static_objects = True + self.sim.recompute_navmesh( + self.sim.pathfinder, + self.navmesh_settings, + ) + + 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( + "--target-object", + type=str, + help="object file to load.", + ) + parser.add_argument( + "--col-obj", + default=None, + type=str, + help="Collision object file to use.", + ) + 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.target_object + 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 = args.target_object + + # load JSON once instead of repeating + mm = habitat_sim.metadata.MetadataMediator() + mm.active_dataset = sim_settings["scene_dataset_config_file"] + + 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 + 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, mm).exec() diff --git a/examples/spot_viewer.py b/examples/spot_viewer.py new file mode 100644 index 0000000000..1996dd9104 --- /dev/null +++ b/examples/spot_viewer.py @@ -0,0 +1,1221 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import 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 magnum as mn +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.classes import ObjectEditor, SemanticDisplay +from habitat_sim.utils.namespace import hsim_physics +from habitat_sim.utils.settings import default_sim_settings, make_cfg + +# This class is dependent on hab-lab +from habitat_sim.utils.sim_utils import SpotAgent + + +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 = 512 + + # 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, + key.Q: False, + key.E: False, + } + + # 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() + # whether to render window text or not + self.do_draw_text = True + + # 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 + + # 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.last_hit_details = None + self.removed_clutter: Dict[str, str] = {} + + self.navmesh_dirty = False + self.removed_objects_debug_frames = [] + + # mouse raycast visualization + self.mouse_cast_results = None + self.mouse_cast_has_hits = False + + self.reconfigure_sim() + + # Editing + self.obj_editor = ObjectEditor(self.sim) + + # Semantics + self.dbg_semantics = SemanticDisplay(self.sim) + + # create spot right after reconfigure + self.spot_agent = SpotAgent(self.sim) + # set for spot's radius + self.cfg.agents[self.agent_id].radius = 0.3 + + # compute NavMesh if not already loaded by the scene. + if self.cfg.sim_cfg.scene_id.lower() != "none": + self.navmesh_config_and_recompute() + + self.spot_agent.place_on_navmesh() + + self.time_since_last_simulation = 0.0 + LoggingContext.reinitialize_from_env() + 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 draw_contact_debug(self, debug_line_render: Any): + """ + This method is called to render a debug line overlay displaying active contact points and normals. + Red lines show the contact distance along the normal and yellow lines show the contact normal at a fixed length. + """ + yellow = mn.Color4.yellow() + red = mn.Color4.red() + cps = self.sim.get_physics_contact_points() + 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 + 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 + 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, + ) + 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. + """ + debug_line_render = self.sim.get_debug_line_render() + 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(debug_line_render) + # draw semantic information + self.dbg_semantics.draw_region_debug(debug_line_render=debug_line_render) + + if self.last_hit_details is not None: + debug_line_render.draw_circle( + translation=self.last_hit_details.point, + radius=0.02, + normal=self.last_hit_details.normal, + color=mn.Color4.yellow(), + num_segments=12, + ) + # draw selected object frames if any objects are selected and any toggled object settings + self.obj_editor.draw_selected_objects(debug_line_render=debug_line_render) + # draw highlight box around all objects + self.obj_editor.draw_box_around_objs(debug_line_render=debug_line_render) + # mouse raycast circle + if self.mouse_cast_has_hits: + debug_line_render.draw_circle( + translation=self.mouse_cast_results.hits[0].point, + radius=0.005, + color=mn.Color4(mn.Vector3(1.0), 1.0), + normal=self.mouse_cast_results.hits[0].normal, + ) + + self.draw_removed_objects_debug_frames() + + 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() + 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( + self.time_since_last_simulation, 1.0 / self.fps + ) + # move agent camera based on input relative to spot + self.spot_agent.set_agent_camera_transform(self.default_agent.scene_node) + + 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() + if self.do_draw_text: + 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["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 + + 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) + + # #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( + "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) + + # 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." + + 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[Application.KeyEvent.Key.key, bool] = self.pressed + # Set the spot up to move + self.spot_agent.move_spot( + move_fwd=press[key.W], + move_back=press[key.S], + move_up=press[key.Z], + move_down=press[key.X], + slide_left=press[key.Q], + slide_right=press[key.E], + turn_left=press[key.A], + turn_right=press[key.D], + ) + + def save_scene(self, event: Application.KeyEvent, exit_scene: bool): + """ + Save current scene. Exit if shift is pressed + """ + + # Save spot's state and remove it + self.spot_agent.cache_transform_and_remove() + + # Save scene + self.obj_editor.save_current_scene() + + # save clutter + if len(self.removed_clutter) > 0: + with open("removed_clutter.txt", "a") as f: + for obj_name in self.removed_clutter: + f.write(obj_name + "\n") + # clear clutter + self.removed_clutter: Dict[str, str] = {} + # whether to exit scene + if exit_scene: + event.accepted = True + self.exit_event(Application.ExitEvent) + return + # Restore spot at previous location + self.spot_agent.restore_at_previous_loc() + + 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: + # If shift_pressed then exit without save + if shift_pressed: + event.accepted = True + self.exit_event(Application.ExitEvent) + return + + # Otherwise, save scene if it has been edited before exiting + self.save_scene(event, exit_scene=True) + return + elif key == pressed.ZERO: + # reset agent camera location + self.spot_agent.init_spot_cam() + + elif key == pressed.ONE: + # Toggle spot's clipping/restriction to navmesh + self.spot_agent.toggle_clip() + + elif key == pressed.TWO: + # Match target object's x dim + self.navmesh_dirty = self.obj_editor.match_x_dim(self.navmesh_dirty) + + elif key == pressed.THREE: + # Match target object's y dim + self.navmesh_dirty = self.obj_editor.match_y_dim(self.navmesh_dirty) + + elif key == pressed.FOUR: + # Match target object's z dim + self.navmesh_dirty = self.obj_editor.match_z_dim(self.navmesh_dirty) + + elif key == pressed.FIVE: + # Match target object's orientation + self.navmesh_dirty = self.obj_editor.match_orientation(self.navmesh_dirty) + + elif key == pressed.SIX: + # Select all items matching selected item. Shift to include all currently selected items + self.obj_editor.select_all_matching_objects(only_matches=not shift_pressed) + + elif key == pressed.H: + # Print help text to console + self.print_help_text() + + 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.LEFT: + # Move or rotate selected object(s) left + self.navmesh_dirty = self.obj_editor.edit_left(self.navmesh_dirty) + + elif key == pressed.RIGHT: + # Move or rotate selected object(s) right + self.navmesh_dirty = self.obj_editor.edit_right(self.navmesh_dirty) + + elif key == pressed.UP: + # Move or rotate selected object(s) up + self.navmesh_dirty = self.obj_editor.edit_up( + self.navmesh_dirty, toggle=alt_pressed + ) + + elif key == pressed.DOWN: + # Move or rotate selected object(s) down + self.navmesh_dirty = self.obj_editor.edit_down( + self.navmesh_dirty, toggle=alt_pressed + ) + + elif key == pressed.BACKSPACE or key == pressed.Y: + # 'Remove' all selected objects by moving them out of view. + # Removal only becomes permanent when scene is saved + # If shift pressed, undo removals + if shift_pressed: + restored_obj_handles = self.obj_editor.restore_removed_objects() + if key == pressed.Y: + for handle in restored_obj_handles: + obj_name = handle.split("/")[-1].split("_:")[0] + self.removed_clutter.pop(obj_name, None) + + self.navmesh_config_and_recompute() + else: + removed_obj_handles = self.obj_editor.remove_sel_objects() + if key == pressed.Y: + for handle in removed_obj_handles: + # Mark removed clutter + obj_name = handle.split("/")[-1].split("_:")[0] + self.removed_clutter[obj_name] = "" + self.navmesh_config_and_recompute() + + elif key == pressed.B: + # Cycle through available edit amount values + self.obj_editor.change_edit_vals(toggle=shift_pressed) + + elif key == pressed.C: + # Display contacts + self.contact_debug_draw = not self.contact_debug_draw + log_str = f"Command: toggle contact debug draw: {self.contact_debug_draw}" + if self.contact_debug_draw: + # perform a discrete collision detection pass and enable contact debug drawing to visualize the results + # TODO: add a nice log message with concise contact pair naming. + log_str = f"{log_str}: performing discrete collision detection and visualize active contacts." + self.sim.perform_discrete_collision_detection() + logger.info(log_str) + + elif key == pressed.G: + # cycle through edit modes + self.obj_editor.change_edit_mode(toggle=shift_pressed) + + elif key == pressed.I: + # Save scene, exiting if shift has been pressed + self.save_scene(event=event, exit_scene=shift_pressed) + + elif key == pressed.J: + # If shift pressed then open, otherwise close + # If alt pressed then selected, otherwise all + self.obj_editor.set_ao_joint_states( + do_open=shift_pressed, selected=alt_pressed + ) + if not shift_pressed: + # if closing then redo navmesh + self.navmesh_config_and_recompute() + + elif key == pressed.K: + # Cycle through semantics display + info_str = self.dbg_semantics.cycle_semantic_region_draw() + logger.info(info_str) + elif key == pressed.L: + # Cycle through types of objects to draw highlight box around - aos, rigids, both, none + self.obj_editor.change_draw_box_types(toggle=shift_pressed) + + 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.spot_agent.place_on_navmesh() + 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.warning("Warning: recompute navmesh first") + elif key == pressed.P: + # Toggle whether showing performance data on screen or not + self.do_draw_text = not self.do_draw_text + + elif key == pressed.T: + self.remove_outdoor_objects() + + elif key == pressed.U: + # Undo all edits on selected objects 1 step, or redo undone, if shift + if shift_pressed: + self.obj_editor.redo_sel_edits() + else: + self.obj_editor.undo_sel_edits() + + elif key == pressed.V: + # Duplicate all the selected objects and place them in the scene + # or inject a new object by queried handle substring in front of + # the agent if no objects selected + + new_obj_list, self.navmesh_dirty = self.obj_editor.build_objects( + self.navmesh_dirty, + build_loc=self.spot_agent.get_point_in_front( + disp_in_front=[1.5, 0.0, 0.0] + ), + ) + if len(new_obj_list) == 0: + print("Failed to add any new objects.") + else: + print(f"Finished adding {len(new_obj_list)} object(s).") + + # 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 calc_mouse_cast_results(self, screen_location: mn.Vector3) -> None: + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(screen_location)) + mouse_cast_results = self.sim.cast_ray(ray=ray) + self.mouse_cast_has_hits = ( + mouse_cast_results is not None and mouse_cast_results.has_hits() + ) + self.mouse_cast_results = mouse_cast_results + + 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: + shift_pressed = bool( + event.modifiers & Application.InputEvent.Modifier.SHIFT + ) + alt_pressed = bool(event.modifiers & Application.InputEvent.Modifier.ALT) + self.spot_agent.mod_spot_cam( + mse_rel_pos=event.relative_position, + shift_pressed=shift_pressed, + alt_pressed=alt_pressed, + ) + + 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() + mod = Application.InputEvent.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + # alt_pressed = bool(event.modifiers & mod.ALT) + self.calc_mouse_cast_results(event.position) + + # select an object with RIGHT-click + if physics_enabled and self.mouse_cast_has_hits: + mouse_cast_hit_results = self.mouse_cast_results.hits + if event.button == button.RIGHT: + # Find object being clicked + obj_found = False + obj = None + # find first non-stage object + hit_idx = 0 + while hit_idx < len(mouse_cast_hit_results) and not obj_found: + self.last_hit_details = mouse_cast_hit_results[hit_idx] + hit_obj_id = mouse_cast_hit_results[hit_idx].object_id + obj = hsim_physics.get_obj_from_id(self.sim, hit_obj_id) + if obj is None: + hit_idx += 1 + else: + obj_found = True + if obj_found: + print( + f"Object: {obj.handle} is {'Articulated' if obj.is_articulated else 'Rigid'} Object at {obj.translation}" + ) + else: + print("This is the stage.") + + if not shift_pressed: + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(obj) + elif obj_found: + # add or remove object from selected objects, depending on whether it is already selected or not + self.obj_editor.toggle_sel_obj(obj) + + 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 + self.spot_agent.mod_spot_cam( + scroll_mod_val=scroll_mod_val, + shift_pressed=shift_pressed, + alt_pressed=alt_pressed, + ) + + 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 = self.cfg.agents[self.agent_id].radius + 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.append((ao, 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 + for ao, ao_orig_motion_type in ao_motion_types: + ao.motion_type = ao_orig_motion_type + + 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): + # 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] + + sensor_type_string = str(sensor_spec.sensor_type.name) + sensor_subtype_string = str(sensor_spec.sensor_subtype.name) + edit_string = self.obj_editor.edit_disp_str() + 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_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. + """ + 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. + RIGHT: + Select an object(s) to modify. If multiple objects selected all will be modified equally + (+SHIFT) add/remove object from selected set. Most recently selected object (with yellow box) will be target object. + WHEEL: + Zoom in and out on Spot view. + (+ALT): Raise/Lower the camera's target above Spot. + + +Key Commands: +------------- + esc: Exit the application. + 'h': Display this help message. + 'p': Toggle the display of on-screen data + + Spot Controls: + 'wasd': Move Spot's body forward/backward and rotate left/right. + 'qe': Move Spot's body in strafe left/right. + 'zx': Move Spot's body up and down (PROTOTYPE) + + '0': Reset the camera around Spot (after raising/lowering) + '1' : Disengage/re-engage the navmesh constraints (no-clip toggle). When toggling back on, + before collision/navmesh is re-engaged, the closest point to the navmesh is searched + for. If found, spot is snapped to it, but if not found, spot will stay in no-clip + mode and a message will display. + + Scene Object Modification UI: + 'g' : Change Edit mode to either Move or Rotate the selected object + 'b' (+ SHIFT) : Increment (Decrement) the current edit amounts. + - With an object selected: + When Move Object Edit mode is selected : + - LEFT/RIGHT arrow keys: move the object along global X axis. + - UP/DOWN arrow keys: move the object along global Z axis. + (+ALT): move the object up/down (global Y axis) + When Rotate Object Edit mode is selected : + - LEFT/RIGHT arrow keys: rotate the object around global Y axis. + - UP/DOWN arrow keys: rotate the object around global Z axis. + (+ALT): rotate the object around global X axis. + - BACKSPACE: delete the selected object + - 'y': delete the selected object and record it as clutter. + - Matching target selected object (rendered with yellow box) specified dimension : + - '2': all selected objects match selected 'target''s x value + - '3': all selected objects match selected 'target''s y value + - '4': all selected objects match selected 'target''s z value + - '5': all selected objects match selected 'target''s orientation + + '6': Select all objects that match the type of the current target/highlit (yellow box) object + + '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. + + 'j': Modify AO link states : + (+SHIFT) : Open Selected/All AOs + (-SHIFT) : Close Selected/All AOs + (+ALT) : Modify Selected AOs + (-ALT) : Modify All AOs + + 'l' : Toggle types of objects to display boxes around : None, AOs, Rigids, Both + + 'u': Undo a single modification step for all selected objects + (+SHIFT) : redo (TODO) + + Utilities: + 'r': Reset the simulator with the most recently loaded scene. + ',': Render a Bullet collision shape debug wireframe overlay (white=active, green=sleeping, blue=wants sleeping, red=can't sleep). + 'c': Toggle the contact point debug render overlay on/off. If toggled to true, + then run a discrete collision detection pass and render a debug wireframe overlay + showing active contact points and normals (yellow=fixed length normals, red=collision distances). + 'k' Toggle Semantic visualization bounds (currently only Semantic Region annotations) + 'n': Show/hide NavMesh wireframe. + (+SHIFT) Recompute NavMesh with Spot settings (already done). + (+ALT) Re-sample Spot's position from the NavMesh. + + + 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( + "--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="Enable horizon-based ambient occlusion, which provides soft shadows in corners and crevices.", + ) + 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=1080, + type=int, + help="Horizontal resolution of the window.", + ) + parser.add_argument( + "--height", + default=720, + 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["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"] = args.hbao + + # start the application + HabitatSimInteractiveViewer(sim_settings).exec() diff --git a/examples/tutorials/nb_python/ECCV_2020_Advanced_Features.py b/examples/tutorials/nb_python/ECCV_2020_Advanced_Features.py index e2b04a58b8..cff7459bd1 100644 --- a/examples/tutorials/nb_python/ECCV_2020_Advanced_Features.py +++ b/examples/tutorials/nb_python/ECCV_2020_Advanced_Features.py @@ -171,7 +171,11 @@ def build_dict_of_PhyObj_attrs(phys_obj_template): False, "boolean", ) - res_dict["is_dirty"] = (phys_obj_template.is_dirty, False, "boolean") + res_dict["filenames_are_dirty"] = ( + phys_obj_template.filenames_are_dirty, + False, + "boolean", + ) return res_dict diff --git a/examples/tutorials/nb_python/asset_viewer.py b/examples/tutorials/nb_python/asset_viewer.py index 800996632a..fc7aacb6fa 100644 --- a/examples/tutorials/nb_python/asset_viewer.py +++ b/examples/tutorials/nb_python/asset_viewer.py @@ -331,7 +331,11 @@ def build_dict_of_PhyObj_attrs(phys_obj_template): "boolean", ) res_dict["is_collidable"] = (phys_obj_template.is_collidable, True, "boolean") - res_dict["is_dirty"] = (phys_obj_template.is_dirty, False, "boolean") + res_dict["filenames_are_dirty"] = ( + phys_obj_template.filenames_are_dirty, + False, + "boolean", + ) return res_dict diff --git a/examples/tutorials/notebooks/ECCV_2020_Advanced_Features.ipynb b/examples/tutorials/notebooks/ECCV_2020_Advanced_Features.ipynb index ba236a60b4..4c70621a03 100644 --- a/examples/tutorials/notebooks/ECCV_2020_Advanced_Features.ipynb +++ b/examples/tutorials/notebooks/ECCV_2020_Advanced_Features.ipynb @@ -163,7 +163,11 @@ " False,\n", " \"boolean\",\n", " )\n", - " res_dict[\"is_dirty\"] = (phys_obj_template.is_dirty, False, \"boolean\")\n", + " res_dict[\"filenames_are_dirty\"] = (\n", + " phys_obj_template.filenames_are_dirty,\n", + " False,\n", + " \"boolean\",\n", + " )\n", " return res_dict\n", "\n", "\n", diff --git a/examples/tutorials/notebooks/asset_viewer.ipynb b/examples/tutorials/notebooks/asset_viewer.ipynb index 1d7e244125..b482cf35f0 100644 --- a/examples/tutorials/notebooks/asset_viewer.ipynb +++ b/examples/tutorials/notebooks/asset_viewer.ipynb @@ -329,7 +329,11 @@ " \"boolean\",\n", " )\n", " res_dict[\"is_collidable\"] = (phys_obj_template.is_collidable, True, \"boolean\")\n", - " res_dict[\"is_dirty\"] = (phys_obj_template.is_dirty, False, \"boolean\")\n", + " res_dict[\"filenames_are_dirty\"] = (\n", + " phys_obj_template.filenames_are_dirty,\n", + " False,\n", + " \"boolean\",\n", + " )\n", " return res_dict\n", "\n", "\n", diff --git a/examples/urdfFileNames.txt b/examples/urdfFileNames.txt new file mode 100644 index 0000000000..2488446f03 --- /dev/null +++ b/examples/urdfFileNames.txt @@ -0,0 +1,2012 @@ +0a5df6da61cd2e78e972690b501452152309e56b, False +0a7cedd2c534592ab5d5be1f0d8979e47896252b, False +0a9cbe043deabb9971a124ea7bdd0f1d242152c9, False +0a9e33bd8772e0b3030d0a443b990328141c7028, False +0a66e9d231f702eec5e2e29781a24859b1ffed9f, False +0a516d39b100499cfde1b2f7071930bbe6c61cea, False +0aa5adf65e773a0d08aa9147a39b8c325bf32424, False +0aa9676e3e5844f42b38e3a3b40dc70515fdd84c, False +0ab4c29164cb03cc33de6ac67677765eaee85cc2, False +0ac0bc57b0ac6ed9925ae77a5cbe4db447c89e6b, False +0ad85902da0211d4439630d8f217340abdcfff2d, False +0adf429add0b1ddd95ec48d9f3e644b558e5f355, False +00b0d5e167ae6b42666de010025efad4506563f1, False +0b5c33dc9b1ff7ac340fd431b272c63926560037, False +0b05fd5482a51a5a786ff5df9713b07650a62155, False +0b458bbd4f94efdade07afeb152a5b6cd7f95cd8, False +0bb1cdb98fbdfda5b41abaa39aa7f82321e58b72, False +0bd260f8efb1afe4ec095ef6dbe503cd1f127af8, False +0bfc35ff2d2697e99670084cb923ac6097760b5f, False +0c9ff2144462d68fcab6a79e7737b7307e1a32d5, False +0c42ee5b635c8acdb6bf235ff9420d742a16fd30, False +0c01883f5728e4840e7b57cf6670adb5212572db, False +0c2315f49c061def72461bb659cf5af7dd179a21, False +0c5021dbf2a828505a3c8230ab3f55572b00513a, False +0c5661f4da12434fca0546701f1b00c22edd563f, False +0cde4e2c29781dd8ba67ab0713b684c17cb625a3, False +0ce28476fc30a341e8b5bacb71bff407bd3d924f, False +0d287197003b912092dac121c61c7a1095b5b5eb, False +0da92a74866b0bb0459528ad790edb7353a209a2, False +0db565d684b7c6ca584a63184869a4077bbd7155, False +0dc59d12639006c51fc9289a5030767042231059, False +0ddc4b3e49ac2e4a0432cc84dcc7dfd4033b4b97, False +0de5288928717b30ef111c44a6af982247c279b6, False +0df79a116fc0b650af57895b8c76d70b91dec6af, False +0dfabed4818c34cbaa5ef41a3bcaf89177744b2c, False +0e01fbb8cba5e79ca1de3509b2fa8036f5196eb5, False +00e4e059a54461f2a2789abe85d78e5407d4f8e9, False +0e6f903a9fd3dc5d57660907bc2614428bc52196, False +0e60c694ae2a51c76a182872cbefebf9e7226175, False +0e149dc0640ef51aa60654a574007d0cbc13dcdc, False +00e388a751b3654216f2109ee073dc44f1241eee, False +0e4964ef31001f71a18b6c08cf173d0f636fae6b, False +0ea4b67131a00b8273dd4ac308aa5d11b840a99c, False +0ec98b6b495a298726470f1636049f8e4e4c2926, False +0ef1a02f7d1fb67e00273b14b2dcfea7b02498f2, False +0eff341a995f04ce4b18c56ec30f12d0e9398957, False +00f5afbca9f9bb5eadf718e81a66f0973ed67898, False +0f38ea137f95af6829a411432627e3272fa3906a, False +0f86b100b696d31c886b1896c52e5c47e28c3e5c, False +0f587b130202742338c4836b425cd9478fb971e1, False +0fbee28e85b12216d22db31c726149b9ab8f8746, False +1a83f76b3e1e0d00521c4cc2c791e94ae7af4f5a, False +1a570da5ae0c4d1d51c94be81d7248f155a0dcef, False +1a8882095aa1aa7e9d4ab68d4b36533f833f6d7b, False +1aebe7d2500bfbcb0c6ec787a98a2b3701099ed7, False +1b3a5dd7d2a94add4a978d2516e7158980da0cb2, False +1b5ab65a5923964ddcac70c0d91327c9c370c4d6, False +1b7f8e7466fd35725ff1b8eff84e66bd90875a4f, False +1b39ee78aeac5aa29a473a044e03e6fd4dd2deb4, False +1b83d02f7e17f92c8bcf04b840e40fd42684abce, False +1b4359cec4e8321f0477daa2981fad83b5379f39, False +01b65339d622bb9f89eb8fdd753a76cffc7eb8d6, False +1b414727f2af7f6ba339777ef5be6fcec518daf6, False +1bb66668829970ef66295bb7e8b946b6974b55ca, False +1bccaa8c2a6c0b6f918392ab2adce3c4cc57b2c4, False +01be253cbfd14b947e9dbe09d0b1959e97d72122, False +1bf0baeb5c66c7be8dac14edeb36dc97479cb462, False +1c0bbc026e76c09885dc5c6f156a6c3dec605d10, False +1c3ffde96647f308130ca9ab026b50136f033071, False +1c33bd447d70d4d22116c434d912f1fae78e02b7, False +1c7874f93ca418d7edebd150a7422095fd76897a, False +1c7877df0e45e76bfda344feba4b749e42f2a770, False +1c2452093ccc9db3fca55e8ca07ca11e3e921d5f, False +1cc51b96a5050214f2955649971fe990da9a449d, False +1ccf240adc43432e3bad29950a96f18cfb8bd550, False +1cdf81137f07b14e94171b6e5edd832e61c9449d, False +1cf00d2a74525414edf2680768e760ac5299fe09, False +1d0c63d3d5d2c01cb321c9bd2a0fdc26f34ee6fc, False +1d0eace356e1cdddd761a9aca1d6cfcc9685de42, False +01d1b7a2c92f97d248ca72d85d828f930e472064, False +1d1c44e50bd959ab596f0fa3adbe72c7b9fd4004, False +1d5a78b46d32bf41584c800a0dfa2536d7f0b395, False +1d15c6172a86bc8729d2523ec7fed0fa0c991fb3, False +1d33e8a178a08bdc92720ded8e08125c58a17925, False +1d38f7b0032aea0e6419d5041cac9d8bedc7dffb, False +1d61b187e6264d5010ce83d371c4c46c9fadfa45, False +1d87ec28047abb0e46924b8b8b50931a06ec77ed, False +1d95eadf60b29d6d14d1969463abed7ab31ff800, False +1d01396c2e559fabf007f7ed384055a63a23fa95, False +1da487fd6fe4cd92a48edc8ac44102a8bef489c5, False +1df8886bf3b249e75336ee557458ab673766a06c, False +1e1c18c687ffc14eb4267863fc389ef74c2e26e3, False +1e49c026c853994399bb13af288dfdbfd6deb7d8, False +1e63c1226e6d803139b72b5203c5dd82e083404a, False +1e414b53eef039690faed7557721dd9ce784ef8c, False +1e708a7d0bacf0008fa26b2bebdc0e3701238cc8, False +1eb5c019fc5a80989e5978dab45a8237165a5c52, False +01eb64cfe0595d98521831a9066e146589d90567, False +1f0e602ca3b6f39575b8448db5d7e2c27101e7ae, False +1f1d1126a8fa29130c20424a26176a043af934a0, False +1f3cb2b9ca2bbeab98947de6f076ac212474c494, False +1f6d3a2b8b20f0a07d2f4d02ad79d8697ab15e8e, False +1f8c626c03273ba8a3a181e17d4dd858f7a3f953, False +1f683d9d9451e5347cc742a539e94468a18e9a7c, False +1f48206dca3babea534ced6c1b440a24ed1d8bc1, False +1fa3dec03ab0afb0749373b2c8da8bf77c92e271, False +1ffa630662e8feac26e3f7f6c41f8afc66a81928, False +2a2e5476494b8f8343cf7df02cdc92a1a003d67e, False +2a4a477c158a00d17564b261e44a4f5c9765fe86, False +2a25fe7eebf70b970ecf8c919e5e7391a7392486, False +2a16223055b05a112ca63df1b5c10ca65ce31ef2, False +2aa6ddd969d37db340725a8fcf11d95df87023f6, False +2ab4eaf7e2f72944ae9320afbb703df265f49e9e, False +2abf25b6bcf58df45522eb0137449ef581e7a66a, False +2abfe2c185d18cf31e3dcfd8833207d78daae258, False +2af8f7c0fc1c416b2c24affc510fb8eacef76e23, False +2b08a535c8d8b3353546421c432963c628abb445, False +2b9af0f16b5f1e6b7621bfa05afb45b3581e0736, False +2b39e8c2237ea1f0ba7417b2ec96d2aca51ee17b, False +2b66fdae818f040c80188e978e266d9a93fed57b, False +2b888cc70c8193c8fcb04f4b9cdc1eeb2d82087a, False +2bbec9381d1770ac074d61d4d642ed5ec14c6400, False +2bc66a181f3be064a8053ea0e5a4e464380e2b90, False +2bd4ac31ce87dcbf86d1a1bee10f81d10c42160e, False +2be5f12815e542ec385f7abdb423500e1c90d0cf, False +2be70cb7fe451eabbf070838667e5ae4b8c43974, False +2be038119f4ebc2eb08e4a2ec46607ed9b0c1796, False +2c2b2914fc526f6e8bbe65511ecf58bba0027ec0, False +2c6ab6266f1d11711743b89a00b0f7ff7d64edde, False +2c31adfeb4c1596fba2992437c4ce203633dce21, False +2c69bff136e6399bfc19cfdd8af1d9e5bcbc7843, False +2c95bbd0e11baed1173e4a1c2f34a52f2c0f6dcc, False +2c681d7e64e0410d76156f500cd2df798975a25d, False +2cc02e49ce20336818af4a432db1b0252fce10aa, False +2cdb28938dff1f9ab13aee7630cea51f44f60952, False +2cf4b67bb56ea55fae5db024212ebfe8a3a4b6eb, False +2d2cbcc8a0e199d17277cd6960e81b0fb33d1070, False +2d9c5c6e0cdd6790c5c3b701990826f969b5cf37, False +02d0251f683a3fcd7d5ec09c5a6910ccdb363786, False +2d93837fd0fb80b2f2e9b0d3d55eb6506b22d4c1, False +2d6909175059e49770d08fdd329e8625ed3c9b7a, False +2dc4fc2ba44e670900db84b7fe9e7e98c8c7b338, False +2dc47e88f58c41d9328d2ee3cadad16a4f5ce3ee, False +2e3ac28c9f98387063c77f0cf14ab37ef19b6b01, False +2e3ef297e42fa497a75d5ded416224d80c7d7264, False +2e32ff526dbeb03092164450a309d084a99879cd, False +2e52570bc94dbd4720e59b6c7cd503396e40b6c5, False +2e760269dfbce48c95a73620a808e3e88b60ade4, False +2ef6a4ea1c424e07e9c839e1a1bb3b33fc5d3e0e, False +2f0a62015cb709fe7f07cee6205d33cc054f6eab, False +2f2b94fd61b77ab8cb3d352347ff10d74db8d70c, False +2f3b05938494691123eda0816da3a88833170c50, False +2f9d7000ad830a8f5cab7de9041a427447b6d6af, False +2f57f45e18369078a87321d01b5d6ab37ff7664f, False +2fcbcfe7e87c076203820c7d02cfcfc73bf13966, False +2fd56ec375511778b87d73040f3b55ba6311aea0, False +2fe9c822c423f699138ade0883af91154ce00094, False +3a1c604565d5fa1f411f3fb437a816094ae69122, False +3a2fd60fc421b402f4bfd365fd2a7accfa6ce4b1, False +3a9f78af8ca7d45294c89244ce9fb37a386b6468, False +3a12fb2ab85d734b4b11bc6f541be9961e7ecd23, False +3a48ae6d5886879be81acaec677cbd8cdbd3f2db, False +3a80d608e8af86e86f62cabe698e39e2a5d074e7, False +3a224ace6e35016e51d87d469796ada73c63668b, False +3a5612aee60217931565e5003121a9c98fdbc515, False +3ab3c52f671a0d7079eb20c05538265054549064, False +3acce18e8f27fd9f67c0f3579f821caf5e719d88, False +3ae355dbebacf0ccf273b4af46e0e640eeb4a87e, False +3aef5476fb1c9b2f3cae1b2ca8b33c1bae7ec1d2, False +3afc8b6d8935dc7a3cf7003c4ed073ce13a9bce8, False +3b88f9fe4c836c164705beafa8d5df338c8dc455, False +3b558ce715c88cca63b307f8e0e9b665ca57ef43, False +3b3764c8e5286fcf2d922e6af05057af8d3a71da, False +03ba174d13db7f909a79cb64d4319383c6fa99ee, False +3bbc952ca877b13b41254bc345b1416eb557d4b3, False +3bc24e0ea79ade13d4703ca23ece1e4019f9b70b, False +3bcac30fb6c85be424262960b8909586dc73731b, False +3c4cc7430793a8ec2d253afff99a37124395b347, False +03c6a0317265df8eeccc9245675fc50af95072d5, False +3c22ce5ae9be5492c9816faf85879f48534af732, False +3c37a58988da0e601a092ad2407b45810dae2b04, False +3c080d106ad03db9be1ea6c996264fb66c053973, False +3cc3f05834221a1ce65063cd7ace3d7ee88879cf, False +3cc5cebee40bf8d2bfe4042b1f693e4eeccc54a8, False +3d9e9e5d1cbdf437128cc3de4b97ae6a404fc0bb, False +3d71cae6d28e2b2358f25de31ac1b03977654acc, False +3d3065db31b26d506142a2d7d6a6fa0baba0e496, False +3d3643adc7e0934142ba5482adbe691056196e3b, False +3dc65c3057a59020426aa9746a85d9efc3dadc40, False +03e0a5b0643512a63c10c478fb005fb1228e2ccf, False +3e4ff11c4eba7495b101a6a1eeec8480520fd3ec, False +3e89c436ad75242425fbe429e7904e1adfd3a67d, False +3e8468e6a7c286c61e1ceb5f1ecbec77f07c1e38, False +3e40038f7ac311b80e217680e2db1043f3ac12ec, False +3e52854c430f559dd400e7272105ff174b44b6b8, False +3e4790548c158671fec162757053201da04b6259, False +3e436779733ba0d9aca4a7c86d2ea08f99c2eb98, False +03ea6472f528d540163f2067c40e832054ed30ab, False +3efb80a75720ef1848a6ddf5011959653e53b172, False +3f05be6330befcf290382d07a18b48ebc92d991b, False +3f28e55e96c9f14cd017e13cffa16b02e5db452e, False +03f226dd0e012925a8674564c4de19cb786c9a88, False +3f692c319f4d06a270aa586b95aebe8def82f6de, False +3f9265b5b5e5abd5332ce66078e5eecbc3a9366d, False +3f91028247466d85c5fee23c1fd754a9ac42c2ca, False +3fb0ef74ad5000c19e088c6ec09725faf75298cb, False +3fc77979a4622ac665589e79822641ba6b7aa66f, False +3fcbaf139511bf243191f7ef92adde8d5fd7821d, False +03fdc8dd8b89fbff8cf749f5c847180ab13e53d2, False +3fe10518eb7c00d7a58b15a513980e9606373a27, False +4a0dcc762f78bcca846fa2490b5fa65920408411, False +4a14b89d51b3dac595041350de6beb6fc8789d86, False +4a90b2c2645d328f8ea021c3926b19a07b02b9cf, False +4a326efb8ab35d8ce823575d4c3b7033e8c3e5e8, False +4a856d0d050c8748773309117d425a3f6690d07d, False +4a43310877336ce42f4e18c88b4effb3e7a5b7f4, False +4ac5e435efc97b7e2ae23e51e6b043de2276c1df, False +4ac29c0732f6cfe6fd45e2bb2a999f5b33ab6fe0, False +4b4b56c002cd225a167e4bf7a04c767d689ef6a8, False +4b7cdfe287647431cc6e6ffc2cb59aee0f19bdd0, False +4b02240753963be92086f8fb2e1ec3e2a3259e77, False +4ba13bf9c8d4dc2c9edf02da945e3712c47a26b2, False +4ba52fb378a3d5f0e77f42146cc0d9c248479ace, False +4bd1e4ec215403d3239f09517e44568f78da3b40, False +4bd6a489605fc44259928cd5cc6d7942230297d1, False +4c7f0d629c98b1d3ebfed5dc10d49dfdecf0fbe1, False +04c99a0e259fa98fbbc7171112d0a285c1648718, False +4c246a64de4bb9fcce09ff4523db1661f42d2d95, False +4c911c25364d1cbb37493ecfaa6b889d931c78ac, False +4c89823f7c95bc341afa56a871f8f01832f86708, False +4c4122564dc6381c97a10a7c6c75fd260110ff66, False +4c067979055ae739365d340a1699036be5c136c7, False +4cee8b9b69f3cc0d1072af4a633d28046ddb8f83, False +4cf3e3b0f7cff8a68c6193ad6bf8f3d85b5e8908, False +4cf74bc7112b601fe700469637710b150f539a5b, False +4d1f79a428083d4ed582fe7e04b58814e734f3dc, False +4d2c2d793673e1bbfc7139a1247788878e1f3855, False +4d9ef2d3865a38064dcf1a71522ec36d43664507, False +4d13ab43898ce0d21a716edcef91fec02ff8cb2d, False +4d4709a41a152773f55c745958f1af7670dc59b7, False +4d82784c7882172aba10d6a91421211c581758a4, False +4e0a645fa6515ad09308a5243cd77e3975e4c4da, False +4e6dea40f6b6ee089d73536859c167cac7dadb3c, False +4e8e1181584e949cc255e66b0ca57b6bf1a6b49c, False +4e74ef0956614341bd226da209f0fd9798c2bc68, False +4e72153dde72fec8fbe798d7957d2baf86b0547b, False +4e74376ca5c86ab82bd86383f3551ab23f2d6c34, False +4eabd25d882465273f31beb507b5f508cd211c8d, False +4f89b3cd70cb8d85fa7e9f050ea71aed1c3d80cb, False +4f167cad5def9f7fdb4ba74e90d21ba0943445a0, False +4f19667d3471b5c6334c100f58e8028bce855ddf, False +4f33248af82833970062489cad354a22887d0a78, False +4f59765435e64524ba5ac8e25427a895fa530311, False +4ff59e8070d5ea9934152438923cccb3c0a94c20, False +5a5c19684737ded0da7c2811f6303e154a305323, False +5a54f0f70cc6b150f3c4562e65b13f6758508d6a, False +5ad437fd9811ca418f4c277a5f82c57cd7fe6efa, False +5ad40619a3bb78eac62b5d40817966fce8f9814e, False +5afe0e7ec04ac3d20d49e030cafd2af9b5d75b85, False +5b16f3083c6a9446128debffc2cb7c911b54e221, False +5b023d45733513292cf20484687905e14e6bddae, False +5b31f4fb7ff9d142b3d347a19a46d5f7bfa8962b, False +5bbfd9e1325ca8d3c59e23b35401eeec71424256, False +5bc304ca5557edc8a25e7dc579b128100e9c284c, False +5bdf2a4a8a5b31a5e0549d480caed0be6631f110, False +05be6be9816aef09082349fdd55a24aed992f467, False +5be14748099383be5920e984b11ff6f27b700c2e, False +5bea70d6954327c1207b01be49a2bc1676d90bfc, False +5c8e3916350ace42d01695e859d3ecab3ada2934, False +5c60be6945490b8be0256760b447608e49c55150, False +5c576a211fe4d96d92cab5188f0fb64dc96e16e8, False +5c024410b71ba4ffd8be0b31633c7e3069688699, False +5c9209213a1247e2f16d1e1a4de961b2db2816ba, False +5ca9a18202d7cd7fe7568cc7ac588705b4ae4afa, False +5cf876c569c3225eab848d4269c7baa489f85014, False +5d007ea89720572a7ec3460109cb1b3eb1609791, False +5d18d5b98d3bc4f4e6d01768c942596e9c30ad0b, False +5d35a548860afd3162719b65fce1d139e25879a2, False +5d245a16aac5f3dc127e696a75fc04917c1f93eb, False +5dbd3d6e061ae2af0c81af2025304d6cf6dcba58, False +5dc40ae3eeb5fbd9c901ba3bae7f6e4647614c81, False +5dcb85c360fb039caba75a42f9d0e2e41ec32190, False +5dd163c10ad18f02392935bac6fb31738507f9a7, False +05ddc6731f34bdbe1aa5cff98cd8fcd1db254a84, False +5e4ec54ebe33776ef7fe4f8f8a1529a0b0395ac6, False +5e5e9b3a4af489ae0e2844a27ff182856c37ac48, False +5e57d965401fa3e4f8a4691ba4070c7c035274af, False +5e74de078d0d0bd18ecb9f41feb1fd2aef42d478, False +5e60836dcd1cda51a634fc116b9d86eaf884cd28, False +5ecfaebc77f39cc5ae8f043269c88d34457fd9c2, False +5edd951f5377becae36ad4c35e919f6d3a6b3da9, False +5ef3aefc0163cdab3a805b1adac07c0ae89af7d2, False +5f1c51116d658b454806329a7be495f18190e7d1, False +5f7fe56c41db43b1673d70acd0079e9a7ad9c3eb, False +5f27a5439c763328e9addc223ecaaf2d61115a1f, False +5f074f91cc2ce2a4d5a62e6cce77c435e5dbf457, False +05f80a1f59e03f798ba3941f1a6f2712a0e93583, False +5f83bf65164f100a15060c534561322571dcbf6e, False +5f333db79d970c5feeb7faee4cea5c6cd64915f5, False +5f583b3f62d67e33fa89d793c04bfcd87745c696, False +5faa380d76943834db056af31a00b92a71774873, False +5fc90748ff220084949fb755a56e4e497b777e47, False +6a5c779b2deee1af83e7d2f85605fe0a1d39f665, False +6a22a1adcf5cf78d6a81c8fbe03d2c5977b5fba6, False +6a93e49c0231ccbdd29d3050e5e1e8053081b7d0, False +6a194b509df258ccbfac747edb2cd5a93aa5d102, False +6a881b7f3f88afdc4123985d2b5698646146385f, False +6aa6b20a49e05ab2cddb6899406ff752bd8d4e61, False +6aa6ebd03fa90a8cb7e8e7b3877327796a48aa8f, False +6acafe4f37b17ff0c33d22c8dc3fede1bddc67de, False +6b5a67bd25f26e993f07157c34c8cd0dec068ea3, False +6b8855c3db6f28bd69099154eecdaf5df16415ee, False +6b15594c03361d9b67e5cd824aef04613eebb130, False +6b63013dfa25cabaa2ef3d0830c19c823e6b1f7d, False +6b72541c4a983863be92d0c603846ca9e1792ad1, False +6b310298f178aae8a0b23a70e1388cb67c3bbe71, False +6bb6bee19a1dd2034abea1ba84107ef7264897ae, False +6bc64333d6108aae201e5c0bf1ce9be86b7f7fcd, False +6bd2e5696abaa840067ea1349d48cfd9c5d4f17b, False +6c4a24cc7705bcc05b977171a290f2f48d0db5ce, False +6c4db0f4e4980dced9e0e5eb12614bf284a6fb51, False +6c80f089f60961936c04cbc4ef61d2563eb40062, False +6cf8d5e45fba1df742e037fd4cf7b46ee921c643, False +6cfff2f2a9750f3671c84c2f9e01bedc401a6f58, False +6d0a8700e168806df1234300f20a8969b23d0528, False +6d6f941aefa3f1c6702dd367f6827b6de6a944a1, False +6d14e5d8e9e4a77f19b7743a5ad16d44f53e673c, False +6d24b2fdb2fd4ba5a34bb077ce32d05270fb8ad2, False +6d40e74eb0c4a0ee6aa532e66ee080b9506f24e6, False +6d85d1595f0fb31c432ec91cd92e1f7d383a8310, False +6d264e3023b940b0b1d31b77e04d0c845853c1f0, False +6d373958555e97eb77c89ce28fae01475877b0b8, False +6da5d91d0c7b695ec9ed5bc4beb21ab6c483ec0a, False +6da53db5c9730fefc2507f1f76b85f7b4c037985, False +6e6c0f0829627cf5ca77c58142311dcfb12a17e8, False +6e7c674ea7231612862a5bb66960f0cfef5d8f0e, False +6e7d2d9091ed8600596ace93233592b41ef9e58f, False +6e8ab0b6640c5a66914dbb05d4019a5dcaeb0b44, False +6e68da11967d62ee675e49564638459b0bc51297, False +6e74c32e636a3f784d5a1e21f4bd323a56064d3a, False +6e93d82877d829b2f8d80c577dcd6329e66a99f7, False +6e96a82e4a2e30fbc4bdeaee52bf04f66b20a09b, False +6e799bed095680306e2852efbe7f191cc6c3ff87, False +6e5462e6109ec906d288f9f4b8206a338df0c908, False +6ea37d3943f6d8989151be079cb9eaa711a40379, False +6ef98b7750ff80c5225b0f7c733f23d196998038, False +6eff75ec23c4c9c1f83c636bf6d6ea6993d7adde, False +6f0b2844edf4906d69832484e80d5a69d9b33b1e, False +6f2bb312036ebbae505dbdc84cd4316e48736776, False +6f57e5076e491f54896631bfe4e9cfcaa08899e2, False +6f3757cad1e03ebf1cd2407b925ebd58e15f8728, False +6f259794d01a14a54e2eb65e30a661f053abe729, False +6fb03649e30b60b37718bbe01f25e7338248a07d, False +6fe1d19bf2a3d498aff6e18590116fbdc07f3aef, False +7a5b05275dec9acba52fba417a806a20309cda8f, False +7a06b033beac2d46a538e141792ccc507a0ec6b7, False +7a6def1426f562d3c426c0aa5531b0470cb5fb7a, False +7a3152d4a996cb228993a71cc0ef6931582011fe, False +7aad199df0520c5e3626621ed9a45403ca92fd59, False +7abd74b8300c96386861ddd3f62dbca5b4b093a8, False +7aef116305c3962b1e6b3bbbcf3c10636d4b1464, False +7b4ceffe179f3a3c2d03be09ad8686ff1a15360a, False +7b7a40942d8b5c3a32d92157a5f03f3631a08b1d, False +7b1020b9354d709408801b53ce2016c77e4cc4ff, False +07b4946baa48b230b224946422be140ff626c328, False +7bb26091737590dd6c2941936c871bd32a4dd864, False +7bbdbc91b4e39aa072906b2c0c8e1deca281603b, False +7bd11d5fbae45fba89a8e6a180383215add13b69, False +7be32a2ac868bf7cd66021b069f21378eb33a80d, False +7c2cba8733bd076e5316c04aa96c80e05c88d5b5, False +7c9a87b5f09ef11c66f8563598239ffa52e6fe9f, False +7c85d0a371c5be2f5de0883e0c2125bd77de73f5, False +7c9782ef8b9b4b2916233c8ee2180dffedd0b3ad, False +7c135687fb97d18307339aed175e8f3c46d6a378, False +7cba44fcae20074ad3f9c1ad77a7c16060e8949a, False +7cc4261ecb65b58c7dd1e2bc2fed9dd3653ac0f3, False +7cdeb790eb0a2a35f1bd5a29588c1b251dc0a3e4, False +7d1f2631bececebeb2ec101b35ab998fe9cc1506, False +7d01fdf3d6698ff23cd2e97e7342b8e4cf360a23, False +7d126fcaa74f70c6e45cb0f735f040dd21fc730c, False +7d625c430972d6778f120ef87e9e163fad23a778, False +7dd55651012d3cec11a14ef934ef669c71413d3d, False +7e0cb6031bb9e6ce31d41f165ec89a6e71b87e74, False +7e7d4081032a219390bc671fc4a88f2ef0a466ff, False +7e9ae82058f7719b0e8b0e83a055ed7341afe69d, False +7ea8cf3577b6be995849734b877dc5a51510998e, False +7ec8e9feb1d56ad9e0f6a7647d249c652b987031, False +7ed48ee48050b080a2e214f06eeb79d6ee9c17be, False +07ed148d60afdc1f9dd475c10f70920400bb910d, False +7f1ffde09506f5fcb04ce049fd1ef6d1d33101c9, False +7f8a0cd7af3f9752389a3615b62da6d803ef7d67, False +7f30c0812a7a5c37123a88056325ef7aae57150a, False +7f98d7f8b92972a8f65e3584626ac02b2c731931, False +7f52743a06cba973a973a13dae47a572dfa0aadf, False +7faa0391d189e7deb12173a1e79c1992e6511282, False +8a44f5e764ca2a131f50709d1253d341a61282d4, False +8a75b901c9375d1647b9a7d51e6a6838a9751a86, False +8a95d8b56329acf8153705fda06b007240747b03, False +8a2440fd268adf3842f55c58abaa35c474934171, False +8a2467f42dc962b5a3bcc7868f6faf9ea31397dd, False +8aa26e2781417fa9cb7d3a6ffeb82121a8de4e15, False +8abac94c4a656d0de2cbef4e3289fc264534338f, False +8ac4d1dae919d7391b3ee3ff02cde5de46dc30e9, False +8ac7af080ae319af28a2b5cc651f14e6ad72c130, False +8acbdabe5523ae5326804e4472d9c9910c94b4af, False +8ad3eae836fed059c1b6979c96903573bf37e1e2, False +8adc3b058f1684f4de9e4f6d1ca718f4f2e1df4f, False +8adff54532c5b497625e63c05997fa166786f5f3, False +8ae8c09d6a2293f578abe129f16114d6b877fc11, False +8b1f720d6bab266f0442535240773c3520eaba0b, False +8b5ae9acc15bc4d159f983229078ba432c2159f4, False +8b0722bd02e26bf71c7218d68a29d545d90e8746, False +8b63648f4bdf1484b373ee705ebb0e22af222383, False +8bcfaffa5c38da25eb6575908fe19effb10d5b3b, False +8be12bde64c1540adeb4237219dff8a004c442ff, False +8c3c2d10c8fbf3a1e29030dad0366ca29feaa697, False +8c5e34addd948e47b6a0ac28c837c5c418c38ac1, False +8c14fade032205abee0a1ffb662aec4fc70ebbb2, False +08c022d1c4603d259da265a9725b24ce8f5fda07, False +8c3467fa914e8ec851f51faf39d9dcded4864eab, False +8c9040d12b61c83a5708ee399ff38ddd14450e35, False +8c5266145942e091cb28561b65a02042e309c31f, False +8cbd4ea7f6834e2a37ba461dee1139634de71ff6, False +8d16e23a5540351c7903b660f05627fc97aabd0b, False +08d3332bdb327aa2b5ca2b252063795d72345b86, False +8dba4cb9fe62d4f420d087fa9c358a3a7801e404, False +8dcb19215c152b7949611295527ebca080754b6c, False +8dd71afd361b2518e475589f315563da158685aa, False +8def8a8bc0b3e024c288eaa3c13b614af01eb4a0, False +8e0ed91a99dfa774e8e3b49d699bbc8135575dd3, False +8e19e484f5560f614078544788f1a26c83f0440e, False +8e977c12aaa67ac4f8e4298113eb52707904ed60, False +8e507631cfe2defc4bcf7c8b67282c9e99935f7b, False +8e792021f82f67d9662c15328a28063d06dc3623, False +8ead2614ed88bb808e684234eec0f9120f41c089, False +8ecacd660777c8780afad3e1d2244164a9ffc15c, False +8ed2b4fe575e0f888a0ebf7676b4a53be492e89a, False +8ef198585e210a09ad76c8d8ba1d0b2a9d7cd005, False +8f0b7c5d33f0a4f0ec14afd05f1690c6468d7561, False +8f3a6d7c5803dbcfaab0e4acb7c2ae17e49dcffb, False +8f20e048ed6d9a806ecea50c716a4a8847eb302d, False +8f75a5a5837733b9a5beebf031f4cdc13a240782, False +08f5333b1ad04d00dc764656bd47c9cdbd67c00b, False +8f66487e4f6eb9e2e90abf9cef5b4bc6d28cf2b5, False +8fa78d5ee00e7feb9a97ad65ae4264202987b234, False +09a0a4a031e33e738214b812f71dd838232df54c, False +09a06a10e1fb974571bbf56cbf80598ca2dea07b, False +9a7911e5accdc8ade2d4b12e9954d2b74a1fe231, False +9a83914d865d8bcdb7671cc976db131f28b1eec0, False +9a1565251171e586b26e66e387e80a106141d129, False +9aa961137b4a6103e242a6e515aefcb7aa434161, False +9abd8759d66aa8e8bb0de742c0865cc70c057603, False +9adb235d4841962c4ed5729d151cfe3ae39609e9, False +9b0a96db7158b9278fa8fd15c165ec0d2475d1b7, False +09b6f703f78d35692bc3e251c75ccdc131b8f55c, False +09b7f888a08a35d64d05e5f94cde9b75715c9a6d, False +09b8eb37b36847b2dc4f541943befd88b6e74fc1, False +9b34edb22079c93180e57192ed0c919435f8fabc, False +9b64cef587a9b4069ee8e6504715e3ce1a9b1d67, False +9bc019d4d9b1041bb10f625cfb6d45214f1e7122, False +9bf6552ba4cba1cdb6819e60bc3f416807a4eeec, False +9c1eb27dc26b8fa8a45445bbd05e6d3a8998fa84, False +9c7abc8f1dd55d90d9369189c74c058636bd05fc, False +9c88ec4c032a38bfa6c6df7c69a82d3daf694326, False +9c403fc76f1ff83787ae5cfce605d64cf8fad195, False +9c437d74557d8653a9728228ac3b313527ae3be0, False +9c736b82840fea07135e848469e80e60e0f10fdd, False +9ca2f0d06e506af080cb7bf4f27b0b2144ad20e4, False +9ca48c060a1ad1b72a8cb6a7d1d0073985b22461, False +9caeb820d7c3db1172a820e63dbd3896b4913099, False +9ce521b91cc415fd21594aa3c509f3eb48552f86, False +9cecc465b663027d4a59020833d33d85af8f0c37, False +9d8cc5b25b4442b998ed2f009d5e3171d99a2d5b, False +9d6618cb72507fc3e2e656836a32c9960393f522, False +9d7920d6be534f15a3472515d64eac3240dc91c6, False +9da8bbd27b90b21dca1beaa505b34d4426881df4, False +9db21998c823fe0f57702c27e946c248114e075f, False +9dc91eefa1a574dbbe744ee33a1b412f2f85f9bb, False +9e5ba850086f939c461aafac2f8eddc7edb3b1e7, False +9e738f6b2ba6fe3bace3de086e55370ecb4c1a79, False +9e36306b23f0d17e86af758264cf52353a82a7e9, False +9ea4bd481aad181dc67e26e04d97561848ae0615, False +9eb29f8df2d6130cdc5d4028a93ba460688b8042, False +9ed5a2425582f9e6c08fe0ef4286efd6d2d10a3f, False +9f4ddfa2353e1db1588938fbc3dc0a66cb898da7, False +9f89ceca7ee5515295eeb0974b7ea7ba853c350b, False +9f90af0b3547e8495cddac8cfc17811b6bed118e, False +9f95bdd6ed4208eabe16f3c6125519a757aea56f, False +9f329d378c839e9f7fa1ec7c9076c92bcb873f7d, False +9f4304ba9f35ca96c16427bbe90a5ae9f53ea985, False +9fbbbfc58e5cafbc27a31069808cf168cf2d7a78, False +9fe0adbde0ac85b01668517398d8eac2460582f3, False +9fe427f0199ef6414c5e8fe65da17bdc7f058795, False +9fee5e7b92edf3026fc491b252701dfb06ed46cf, False +9ff0e2c2b2d7081e92f2ee4fb21a48a483b5f1d9, False +11c27a0b2950b3a3e9431128cd452ac7cbdf35c1, False +11cf198bb9df9e6a4654550b0952ed6f6769fd95, False +11d4ccea439e533416350bbaffa5325a06eb53b6, False +11ef66ca5682ae7d33e55359edf518ec8fe9cfd7, False +11f8b552a802c6233a1332713568f05f901b725c, False +12f2b36812411b47a61f2a949e82e5127ce04557, False +13cb09aee83d15650e323d09d7f8a1c2fb1da4a4, False +13dbb657414706f4c3ba192d2875ea2c7478b710, False +13fb5e59f1f43a212565ecccb938cef340453b13, False +14a55d7cdd6f72794835d1580ec491b47e16155e, False +14b974e9ef733a0a753ea88b1633551271031a0d, False +14bb88e168278fed38a7511f2a571d32be7922d3, False +14d3159664ea16859019f306ef64b59c18a65f6e, False +15a81c5727eba4e4a4161e1cf2c76fe2c601ae15, False +15ba26b5637f8aad35673ce31e9c839a4ee36fcc, False +15c0bd474aa43ea3156a6d33a23dc58384ac68ba, False +15c9b29a5b9fca8a71064714b0d2c9ba0dab7246, False +15f53f8b627d71783e56db67e71fe075a018959c, False +15fbbb30071be7f734a79e0ad8af6e5797c95d35, False +16a4e41e6085b65ee5f86acabc8c7185d84c4749, False +16b93d86d1a466f5982e60c8d322ddd8f312056e, False +16f99c186c5cbf0cfc342676beb855124ddf2194, False +17a8f7c040254439fe8771f66f1c526c71d24904, False +17a563b45ad4ccdd0ec9e026cacb949657536e48, False +17caf152a2dd87854a20215cbf4652b7933b7cfe, False +17e4cb6839f400d7caa33dfaa11510ff840247a4, False +18c70042058e30b46be5c8f856f215bb78377f2c, False +18cdc18b9629a5f1bfb31f035cfe28007f6540ba, False +18fbff8c769c98fe65f8b37e8db12169ff9b37a9, False +19b181ef136dd9cbbea4505899f9015c58dca260, False +19bc84c6aca89995b69259a0ec179eaa5532ba2b, False +19d770d71094e9160fdcdfec57bed4683afcd1e4, False +20a6b587866d12a14610863ee086d059fd10f687, False +20b40eba263eb20165f26bfc5793feeee9159482, False +20c2307590ca5c2b4f75ab905fc11279d4e8b226, False +20f51e51d23b119cd029083bcbf1c3a01fda8763, False +21bfbe3638af8f9633091dcc16a5c048ac4cb4a0, False +21d8ce05e5b4ff0b35244d2b5f8073cf4d28f04d, False +21fbcf749f7820bee1b2d045d30a0513c717cf46, False +22ece2d4b711becb90ad614ee1b07e937c68646a, False +22f744409072e3a0b1d169995c7ae665b4d505ee, False +22fc9eec57f263e39bce3cd973de66f05cf8cc75, False +23ac7bf19a7773a19f9f31a16815572f93cf62e7, False +23b2e59406a7ecbc76b96b9a9f97714da850d646, False +0024ddad62413ac3089b4efe58b90b01c41123e8, False +24e1d8dfd689b3c978df67dbf5e1596fc512f4ca, False +24f319e7073bdc9de813e228379526cb99406935, False +24f8284e4bdef4397e5b12dc4f2b74a137d63dbb, False +26e2d9294ce1311bf117498e745addc47c703bdb, False +26fbc193d28bac14fb96d7478e389a9925d671bf, False +27a8c993557a0484ff384bb0483d112285f8791d, False +27ac936421bbab724657cde6a7d485cbddb5c02d, False +27b8cec51a8c3b99c079c268d0a184b1f91183c7, False +27d0fcc40f133bddb7ae6ff31aedab20410ffeed, False +27e2fe8d91a9d2cc02ce98e94f5f498548b88a5f, False +27eda9b8daae5b3b6748bc329fe7e312cbd9d232, False +027f20642dd34e7914fc4fc4efa70fbb54bcecbb, False +29a397a12d8afa897fc37a580a5b040632ae4303, False +030dc7920f6b43d809ddc87281cdd46ffdad2d9f, False +30ebb83f48ea03738e4530f17940775b8a251b03, False +31b5ec475d6fdcbb9461561c0a3bbe22dee18e99, False +31c6e463c3d60f9be291c8acfa561bdceaea1cd6, False +31ce9e9df83a6deb8d40ab8bba2df1dfd874e36c, False +31e1f375aa66d6235fbce7b5f34f975bcbe15c90, False +31e02a4cdcb114cdee0b3a6f1454d2551ee2d942, False +31e11f036e69ac95f264368f9736ad533b8c1325, False +31f1e3c48680f2e3d23fd74e870b34a03c4d1f39, False +31f68105b89b6bc24eb8d5e238ed42ae382fc78f, False +032a38c5d0094c30854ad6c58a23ae0767e6bde8, False +32a302104ec8f94faa2950f0167945ed4895e9b9, False +33c9de97bf011b3aad628bbdb9e49021f46bcce4, False +33c1689bc5fb3b6f599f261ad069d9510804deb9, False +33ed9156fb030e849e11ecca7fe95a239f017dff, False +34b1492d454d5339285b02b94d1d3e7833f58b30, False +35cf80fd38ea1668a007ee46d09d4fe94825daa3, False +35effafce676776d73b022654d4bf7c1d668363f, False +35f0ae254da01b15ebcd5f5d7b52410aa7367e6f, False +36a0d67730b7009bbcc7441bb79d084712513138, False +36cb9151e8746f132b466046b29dc649edff0683, False +36d90c43b90a526247d61e709f349b3ed54081e2, False +36e6a6917fdf2b3fccd4b0c82a7bd8b1fdfe9a00, False +37e59e768eb7f42a0019713374063cb63419a44c, False +37f9b1e563de7f6f35d672fa35b842646ce93d34, False +037f34132f162235d80ce46f67c4fa2238d94da0, False +38aad23d1a6ab4834f48025e25ff7143c2f92078, False +038ccdca4c91d34febe3f4efc596225a66a11d79, False +38cd201fd2cfca2f6dd0ee3a83cbae7985f3960c, False +038f37d5b8351b29fdc8173d219ca5a503715ccc, False +39a52ed5a9fa90d42d4da320f129d550ec93fb69, False +39d7c20c71bd2b9e266463c24f2cf8cfdc12d317, False +40effc5a3d4e42ac5778667f5ec82f8019b31833, False +41af0e5250b6f43e286c7e9977a8ea913d4d3128, False +41bafa470b4e4eb9ed3a8fb57a8d4ba5b321a939, False +041c06dd0009364a773620e1eb2fd1dc3808222a, False +41d16010bfc200eb4d71aea6edaf6ad4bc548105, False +41efc8ed9c8d433e9ff877f3b9ea1c0eda45479c, False +42c2fce42ed600de5f0c6919ba47fe97e60ab673, False +42cf6dce36f1ec0dd707f974a10bb95415daa17e, False +43ae5bbfa16a74dbc82d29041846e0b92ccafabc, False +43b7ea81b8b10840845f7d2551599c5a6d96e3d4, False +43dff039cc31d832c3196d9316344c38d9818392, False +43e120bb77cbdb8fadcac1cc33aeacce57e738c5, False +43f8b07b0c8391fb9a457f3096e6a65b5fa8263b, False +43fa24cb165eb3bfde976d070bfd615f41c78028, False +44a3ee33a175f1fdc6fb1cfab6d8c71588e7f24b, False +44b879cb3b61c490ab186f6f9ad8f8d93b9904d8, False +44c41c97eee1e7c7e05b91d58da8d1737586d55d, False +44edc8b97141c3f5ed800be72a5461044c91e829, False +45a35bb581b8132eb57cc8ed646a68f14da6f82d, False +45c662af829bb97af7d5a4068ff83702f072cff6, False +45dd1f636ce3ced4c4bf685bed2e89492dedb268, False +46dd28dc74535f9a2b692ecae0253c9b0e3b40cc, False +47afe6c6fd5e0aebbc942a625f8af8533db131a7, False +47b51d048d81593acb7000ff7f131c5ecb5a4f16, False +47b122b717e177c30cf7bc9e5fde5c6280a5c854, False +47c7faa8f3f007eb9f2ddcadda655ba017949eb9, False +47c0662e06b5b13479b44aca8a3b9094cc99ee46, False +47c28745703eaa9b3bfe792ad97a4de3467d3bc3, False +47ec13733729d7f45b4847f49b76831f092671c3, False +47fa6cf958c4de19ac19ed003cbbcb8d9026ddf2, False +048d80c36ddc6ac63785ca08ccf231431195717c, False +48ee0122414566a9e3a6dd7b1eee41925c38792c, False +49b7ece681efd7ebe1cfc2bdfb80173663358adc, False +49c43df0f4010e965d68f8f748c9810b335ca4a7, False +49fcf2005b74d3a68855cfa604e43778e072349a, False +50d2f90f2ef2466c22a1d1da45fc657763cbfb2a, False +50eb135790c20484ce0067c57c4ff3414312429b, False +51acab78344c2ebc6e223ca8d5f8809a82b55333, False +51b1228d7e9384c9c8b70e2de4a86426b0c2d818, False +51bf4c899c925cae53ee4d4305ac00beaec03a4b, False +51c3494e896f54f12169c9a6a054960048899cdf, False +51ed863b0c679695f7ce6f6843b5ce528ddc52d3, False +51f5784f9aa737d70861e540e5ea988f89c03fd4, False +52dd1f81ee15f3875c117ec2d595c81bb3f9a780, False +52ece22f4c47ace86f4635ce5d3498d1e7a09689, False +53a18ac1b365ac0a7746fa271e4817fb51a52f09, False +53e9a710ba3a592ddf55b3cd2b055003614c4b1c, False +54c594bee3d313ea55f4b08f7ca40cd1be5df5bf, False +54ce1f3c68d8fb0c86ae7e9b88803fc1306044c6, False +54febf97a3c0ee7e010babe4ae9520fbf8858e87, False +54ff8e2cc09fafb0f6b7a8df9f9cf20bd6c34074, False +55da74599e3c3924878594eb35a2106b3a7ea63f, False +55da776747de3474292a65617371432d3cf576ef, False +55dcc18191c2412b899ff405d5d0574571bb81cf, False +55ff9aed203f1ebde9fe8651de7ec26954ed0232, False +56a5ef56af60964c2506afc312fd0f1f83a73d2b, False +056be15536045e9a6a94b9b93cff62f72d43c326, False +56dc6fc7669736b5fd6a85d1b14a01d029beff59, False +56e8ca4a1ce5d5f130c3c9f60add9b07ddd4cc07, False +56f36105d7bf84f7f796bd6272391e16a2e11c5c, False +57ef748a6962b2b7169fa38aaf50293ad64efd57, False +57f69b32d5b69de30730fccc2d15b660f08a6aa8, False +58c1f347bea26244cf39efe7ff10e6cebaf48758, False +59c1d11eeeec63a70985590299043b6a68dd6ca1, False +59d4630af6aee8b19660537466b52f2c26a74c19, False +59fa2c9f7434b3c5351ba1e51914575cc5614cd2, False +61d4155613cdff2e36558a2c210452232eea676f, False +61f1a1e2f7bb45edc46968d3d555a2060c33da84, False +61f3076a83aee8bb3003538da5b94e1e01b1363e, False +62c1e22c2f1fb86794d007e0267535aea29073b9, False +62d5b81040a4546e5fda73df2e6a9648eb6ceb52, False +63b7d75b724e079aab99030084f0eae1b43b7498, False +63c2ea5b388948f5c2625defb37dfd03f9dcecf5, False +063cb879ce734b8ad6367d8a2b0accc69d1b4a7a, False +063e38c2ccedaaa2f0ed59739b30614d2e0d7b29, False +63fb966ae8a96a526619aec4a5a45f81f589a787, False +64abd6658956997ca546c00ebe1e369ffc9bffd4, False +64b9b1d3c8e1484f496fe8b0a566bb1a2d098e19, False +64baca090d527addb7cfa63a881612509fddb178, False +64bc7535db933d4424dcc3e8837d71ad537f4050, False +65d4cb7b229b173e66d4db1bb43b932bd8669aba, False +065ec382b612213e49941b205a074141469eba31, False +65f7bee43fcb222850db45ae67e997889c559edb, False +66b84f2b11792f135e4f0bc746caf3a6feb4ed27, False +66fb8c7cba5203a0326d897a2f314779ee5a9a71, False +67bc354a50314d0a8e1ccc4ec9afad60bc0790ed, False +67cd90e7e00c93901ba366c8ec6f5dbf586890f9, False +67e6eb3d22b09bd2bce2a23fd150ebf234cea76c, False +67e11a52b2e73c099d55405f65f05f7b5141f6da, False +67f44ec0f3168d8f89399b9011294690ce4133fd, False +67f97a6abfbc09c549bd8cf91e54beb16894c595, False +68a08d430e51f231fd0613dbc1e92c75b49f397a, False +68a985ad0150412e7c001933f3ede2acd5622be7, False +68b22165ccec115439ff5bca2a28bd2ebf1fdb9d, False +68caf241969b03395f50ef353b191336a1e72fb9, False +68e0c3ea856e5d85c915e534db8f7ce071012e58, False +69be32b6779a2f4cf9956dcad5be3d51fddad22b, False +69d3d9f343616758471411e4eae82daadc1d1ac0, False +70b446e54a1868313eae5bf4003c9e53306ee039, False +70c22b0935fa1ab0e9fcfe5dd52ddc7ff94f47a7, False +70c79bca253e00b3f33bdca64f8aa12eda31db70, False +70d4947007b0fafdfb7b4fc44a0b556f688ec4c4, False +70da418e165e78eb5933d808fe6a2092162058ea, False +70de3589ff75d49478cbd1f862193de576139ffa, False +71fa3c2594d4399ed3f0d0942e3edcd2be8fdbc5, False +72aed5025bb1eadac274d40613708f1518687978, False +72bea8a7114d39dffad90d7e97a05f7d5fefd3f3, False +72c7b59212d1a6f895b140ec42672f6492776f6c, False +72c7de25ab188d59bd9cd4a3f774e19e97acc95a, False +72c74bf2ba521613b897c442c09b39b8289e9d4c, False +72f27b3eab05de72d5a9808625fe9d02e90d5504, False +73b7a391b1d42c707c01739d3591310d04f3ea12, False +73e0248985913d45c16eec1a7e537c739ea527f6, False +74aeeca76942f11911f77a406caf97034afb2724, False +0074b6bf5758fd7186157fd3a8d53afe3e200cba, False +74c48e1d686c342798a9277975731d178194b288, False +74cfb4efcdf489b79953bdfcfff86d8b42a0190a, False +74da20ef257e10a06164a588ce1e2f80c43f543c, False +075bb144f6398bd53e7d0b16e6edd001763162c5, False +76b32b683454c8dd1ed99443bde39f6021830e9a, False +76c5f5f0347aab6dfa261a6a85c82713f59126db, False +77af27c6a414131f13d2aac55153720028fd4ed5, False +77ca8f9bd244589360ac1175aca4010f83e19914, False +77cbe14c0040514359d35499d3679f86731f23f2, False +78a02cbe18b9bac0aedf264e100a73f34fd05023, False +78a66439eecf2b3387825947e58383ef27e3f66e, False +78c6daa160452aab3cebdbf707a034cf121c86e8, False +78cfa81acee912f151e756369600f7ad48956809, False +78fe75bb14f06cdfba063a60de551a71d84e89f0, False +79bd7ea173156c5e2581f7c3e7caa2bcee9b289a, False +79bf13063599b7fff88cc250d8fe76a2e46e9683, False +79c9b4b08b02cec180072614294b26cec992ca3e, False +79d548e30c2efaba2a41553ef926faece67a5fb6, False +79d5271fc49336b35d5e5bf73cb0cefac692170e, False +80a50e12ec18b3de58081c4285cb6f21938ec76e, False +080af6e022a943b0d3e3aeef330776da6136f17b, False +80b992347863d6cad7230c852749ad51a07fb6be, False +80bfb59e9d68cc3b03a1b04e626640e5d4e4396d, False +80c37ef1455ea5093e5f070d100f9ea7f145ff35, False +80cc694002271fe18b4cb2726af4bab6eaf4ad14, False +80d1cd99216b9f094b07ee445fd33871328aee37, False +80dea841d023ea7ebc15065709eb0cf6b2636b5c, False +80e02f3ee0672344e8ff7f7efb154ecc95489444, False +81b57bd4e7919cbc45157c95f776a9fb5244518e, False +81da9279eae235c3faced51d516e970acdac5e84, False +82ba9f9db1024978e3411c3dfd794a29ac3c98c8, False +82d15c3827173ecdea41382f055a797dc9772e7b, False +83d5a78a042210c917fd7d14d57ac69feb6988f0, False +83f43ff97d3b947794ba3580b2cea7129314d092, False +83fa9995d2ae8d358de0160c9f446ec15048b142, False +83fdaf0b2aea065f371498612b61f12679f1d82b, False +84aa04a73ce6d41924c80ae06dee7761cc4308bf, False +84affc1858745191a1d258858c1f30f45216c8a4, False +84c9b2e52e033c89040e0e0a281b934ae7bfe877, False +84e0cbb4a9b491d4317707dd8cfff68afb397dce, False +84f3e455116910537b312718620493db2c6c5149, False +084ff2a0e018cec0a68d318cc0f37f0b7624c8b8, False +85daa5f12e4407877575d2878ab6b9d00761c6b7, False +85e57bc57bc1929784b066eab7ced82d5f44a6a1, False +86d3381c6bc8822276adfd12268a66b62d74e931, False +87d95877ab352f73ebe9870982d29a5b53a22dae, False +87ef4b11251281db9f644a5adbe6c56be8d5b43e, False +88c87a19b5e883787b5707d90545e25360594822, False +89ba3ca85be9fdeec04468df9a4f75c17d91b01b, False +89f06f94f1c3a8ac2528b60586b621dfabc720a6, False +90bb99385fc88484cb62280e60b6ac330d801f62, False +90bc63fa89c2db5fbaafad4dcc1614e801fcc4b6, False +90fb388959832dd71fdcad95215f28b077fe4a3d, False +91e3292c3524a014023015d2e498f39b4f0cea37, False +092e835bb098736815db26efc98cf8ca90c65dc1, False +93e258bf137542e809e461c612dc358bcfc7df76, False +93ecb08a04a5bfd07faf30edf670664a00b55be2, False +93ed3f069296f4e518625f001dbcc4e451674ba7, False +94bcdbab5bdbd66a83152fb67b620ebbca789fc7, False +94c453a4e763bfec7077df9f80e78909244955f8, False +94f8cc0fc0894bf830f343f98419e5fa2b6fc25d, False +95a48bf28948f4bb669c5858fbdd4fded8da2315, False +95a115ea85acde3b558b0fa25bfd5965dec18280, False +95aa35b1fd2cce0b83d83cc2ba98df60f19d8a04, False +95c4b27e63c13aff86aefbdc47008db3b590455c, False +95c48a9ba1e1531fe26ca7068d11f3e3ebdf178c, False +95c60720df5c0b8d3b59de6ac852f4d1b2d06a3f, False +95eebb59a0118bc955310006181781b67de0303b, False +96aebf7384972662ad6b7e614a7573a122e953ac, False +96ba916d576ea72cad9f317a3013d4bea661e74d, False +96e10274b2d1c6c76568386a781b1d74a226856a, False +96f84bdd96caa3cb6dcc17cc384aa2b1e64b3a85, False +97b54d82ec3e18ffa460e23aa2fd005cc0027179, False +098a8e9aa72ad0663702521445f0673e7760bd9c, False +98dac55bdc009c7efdb01e38c61637edd069b181, False +98dbf856f3251188b0317bdb86c223aaf626bb49, False +98eb241f6120de27b331e1437320d3a0fa751d83, False +99a832c0e386baa03de271a3eb1a0b59de1fb4fa, False +99aff5399a26bd00e6221eb488bac18fa706bcb8, False +99c7b3d9c3c006d2e02e60e3c539c0008af90493, False +99d7270987bccc5133d52f97f185821b7bee789a, False +99e2a3e301a597ed93bf3dc57b36fec3b37b8846, False +99e7fe1527c0c9bcba3a56542adfc3164947c368, False +099e627f0eb0c0f1ae672c25b7df89204dd85521, False +102d6c0b595806db40137663e1314183eba1b641, False +109c5d58f006865af44b50749d94040f2396f754, False +112dc87e26450400941c6eaff60866bc19badc64, False +128b5f2d072869004b7a218ab674f93f73a66670, False +128ed2ced9a101aa0d131fd224012fd52198003f, False +148a13d1dd62547e4fd84e1009f2e9ea1f19076a, False +155c182834f40ebb1d5666d3a72ee828e097097a, False +166b9cdc66cc71ff63ce40db1050166f2fa45845, False +171aadad6db44709e95fe7a95a8d4d01078d402d, False +0175aa74c194cf1c634ee79498cd4afc793badfe, False +183de9f61a9c362d58ac340058ecddc54da18809, False +185df6a6978441ac0370961942d1922af1d234f6, False +186d2c3104f4cc0110c686e2b216c3f20112c63c, False +197a447eb68ba32ab44c50948b3af1e63048e174, False +197d075a8133c29eace426e856c13cfa9cea5b2d, False +206b6f7eb2eb0fb97a97d982380bc470d885fe3a, False +212c762e23660aaf12b153f4ac9d3d9ff4c0f5ce, False +215e08aaca7f96eb917fb32b11a459b2271a31f0, False +221bfe2bb7f6c70688314df7d495359baeb237e4, False +222a445d2cce84b5ad352758634c94962b44153d, False +222ca57c5b35df6477fc8831b271b32fc3755a75, False +223c0f130bcd4b3d546f7044d30ff62d90563177, False +239c5c38a53badc24ca6950ee78d8f6c115c3074, False +243d95c5e20176e0ee7d044a8b197bd95f337815, False +249f9e0c9f1d98925459f980fdc5696c5e5babc5, False +250feb7dc44aa978a6d7bfda1b0fbb61c80e78a0, False +255b9ea318328994923f2d42d79a4b938a731911, False +257b108daedcb3537c6066ed191f8550658a70c2, False +265e819d1f027cde8dac05468a70455f62cbf069, False +266f5506891da94af9540788119afd6a0e5e76f9, False +268fba8564a56219ec24229203fa260753720b5e, False +277a090638f90cdcd36bb8289cbb5aa153032fad, False +277e2a1f66a937da93e3a83443e0dbb7871dd3eb, False +278e27ed94a0c2cd78bc46ed0d347dfa5df9778c, False +280d9f458f11b99e075c23943cbfbfd6509ba4cf, False +283b7c7a82e0fc5b435d6011c2c767ec189f1501, False +291a6b41232b94554e4a613e65fc08bd34274724, False +292e612b94ad5f151ccfea46e97d52bdb6c7e1d1, False +293c42d04758bffcc63070066170cbb1f09918cc, False +294a6acbbf3830bd40e2427777e36ba54d4df3e5, False +297c228c0b9d123b8d31e4b2b871041a092e2e07, False +298fde032f598963af06e258cd91385bb453bc22, False +306a08bd1017e569c2b625d4863cfc2ef7bf904a, False +308f49dcb79e8ec5acb0b5a1ae36eab7c59355d6, False +309eaa78df84ff7241c073ae50de7ae609ff0118, False +311a303f0b3f212b97c5bb23a79a6499efda2f71, False +319c007ee7d07ca84c797a512ad4a98c9abc42da, False +325d98897c49560dee60b4720b4e1944ea2ea63f, False +356ce92bc38493578fdf63b7f3edfaea8001c849, False +0356ddbfa55a90611ee1f6ecbf851dd5b8d3cee9, False +357b5b422e2e657266e48a747a5ecf8ec2c318c8, False +364fd12b7f9dae1470615fb8f459b93c70a94894, False +368d692ad03d2b995e1a75d43369d38247e354b9, False +369ad4b30397935f1ba19e9884bb48707070a307, False +374d659950e5914d44aeca18fb2265b7a22b69f8, False +382eb4073d3aee992000fe0bd80cbf68d0acf182, False +384f050b237868bfeb2fc3bc36075c252bb53e1c, False +392df3314e0d67f75a6d3187f7b4d03c7da28ee5, False +392e00c6db2728b11495753fc32bb9df67e89acb, False +401fef5eb61db2523ccea49ef19f990652e26521, False +406f46db4f76f7973ac743ef7fbc70b77785f05d, False +409aa5478fde8958607258c5974eac6d5336657e, False +409f25c7c976380bb3e7913d3e8e96b8b1eae06c, False +412c833d4428480466b704ce25bf06f519c8cf72, False +416c68a89eda39033eb729fa72f7058802555a9e, False +424d7e7d258370736e6cfdbad2b253e521f80c79, False +426b82af6d87005e0f2fa17217cd45b4826189aa, False +426bfc6d17bdb74313b1960f6e0c4551a7dc767b, False +429de8a53ddf94a47ec553147dc8603b803be056, False +432d534d4c8ff1f6da0a75f2c8d750cccafff7b9, False +437d63141f9c157ca01a57b90e50be01ce2568c1, False +440aa75617a2db12da11307ccfa049f48f76e554, False +448a7a6a5a63f9ec692bcc96e29cf9da999dd05e, False +454daed946130df517b76a1b0a95b873138092de, False +454ed03a4af5b3a58d23ebf1aaadbe9a4968e15d, False +463e89b665479d4a82c4fde27ab4898afa91c33f, False +464d6c2c0f6c8a9cbb9423fd93c8ace7e7f492e5, False +465c339f405bf0f55e0c316820958dee1126dabd, False +474f738976029059759df20b587f138c07803cfd, False +479d975761dac3eaf44a86f1e57cf0927ca17cd5, False +0486ae7dd0b011240a162dc0ff1d96e2cac23dca, False +492bff8222880502aa7b9c8fdd204271605ad310, False +497d4179f9a84aa6b80a99d14be1e606d3ffe2e5, False +523aaef4fc8f251a4bd1ba863647f39a792708c5, False +535dee6b14fd6d65925a82570e4f4db294f73f4c, False +0546a524f6584d5beeaf8a72b3df494f9ceeec90, False +551e75fdb2fce34b3f52933e44fb22d24168a63a, False +556f46d2f7a68af885c2d065e503be422f9a0514, False +559c4ae62912deff99a040dca1861293b6bfa1d7, False +559f21c7f5628a83b31d616e90bdcc02e7744731, False +562cfdfd85a00ffbefbfba9fb6f54e171c99294c, False +578e50f145119f121df79f73f5efd64019d94e17, False +587dcfcb2666a3355ab30cd91929f1a3bd2ed272, False +589ac3bf9116855fa4946be418a2b90da8993a97, False +595c9e233d3e72bb23b7cfa22a5cc3e2523a1350, False +598e90ea5cc487435ec97888b91d239be0abb91f, False +602e95402b712af5ac6273ce4d2d2e6f57229516, False +605aab959303972a631d103fe72d6853ff85b3c3, False +614e0bc7e874a8975b6797433eb9f2a4c49a2256, False +614f9fa85b5783baed560a3123dec8e7deaffa2c, False +620fe798324b52569ea8cc0101a35a7b2464a405, False +622f74d682e7ffa33598568e362f5a0d3cd8a144, False +0625b901fe4510c96ceeec63ca30784cb858b7b0, False +626af6d1f26dc84369a1c192c6da465dd85aae08, False +628d0c20a57798970b7e965946a7bd28267eb3bb, False +632c81dd336ad689bf00f79ff84a7d806ea71d59, False +633b50d9078e02c4df152f2c0faefbdf9c8a53ba, False +638ae5af59935b1bdda9d5543f56505b5f09660c, False +644aecdd8cb3efa146aa57a77d6fe832d952d56e, False +653a4a2592b78e80e1f387554c1d69f0d8465195, False +669c954adf11204f9e88ea0190ad45bc9963c517, False +671a738c995751adca603a8aef3781db5df82e84, False +0673c8be2931d0d95d101732908129d9b607bea6, False +674b628a31866a0ccef740fbb4836d8aca5a5d51, False +674c6288df1fdae7fce61eaeffe0c267a3838de3, False +674d87278bc0cf255fd9607a1dfa43993c3fc4d1, False +675b6f2c9df66234ba50b68814ba0513da4fd0d7, False +678fb510b3c435ee023930904083278c3c67670f, False +679df106227c9c72b36ac24b71f678024893ebfe, False +682a761a9e4192d3f6fac31dcc4975d4928a5d0c, False +0684c04a6a19f88a666c2a541593ac73185eb128, False +688eb70a1afb2467d04303fe35f9a59ebc47acc1, False +692d92348c0088397c997e7aa1e859e2237fb9d4, False +692d3522273f51cb275783d5d2a5dd212960e474, False +698aac1fa524f06b75a0bac01de17c29deabec0a, False +702d3ab650d34ee1dd236b0df5882573ec70c504, False +710fadbb8a88b425d87b6beaaecdb252068b1a7d, False +711ca0050f3f9b5f88d68e5b09d4e176090e64ad, False +720ecb7a8eecd42e4eb0905ceb9d8253f12e170d, False +722d9b8b0d8ad98e8798840b918121d2c126fa26, False +724b0686d3d9a2ee5ad935943bd5ea2a69ec77ce, False +725c5e0c013448a939b748491fe3262b0fb3185e, False +737a79fca8034493d304924c4d4526f73e9b590a, False +738fd4f9d5f988090b5b70b353651e2d9a919a11, False +741c787824bad244f18eba3b24e0b5176631217c, False +750d6a977be6352bf688539b5ccc6e1147b22758, False +757c3b2e6018679a04ef04fc6f21b6434c379ebd, False +0760a3dd43bd9dd9c0ec1ea4e033c6e121d92df5, False +761f0c299a2f5a134f78ba175a0fd1e372a86dc8, False +767bfaf6eaa36c7833dfa253e45802b169653ab6, False +770bb72cc8b7efd32289f74ba7cbb258f82bc39f, False +778b4fb2b33bc168ba059e2c54e853bbe06ecbe4, False +787d60a079f4a470dfea457688019097135a188b, False +792da1654a2adf3da64a91aa8ef328f98074b82c, False +797dfff15715fcc568ce2e6d73c12f829c3637db, False +801ba807a72e0b826d2686614e01353d2ef43e2c, False +806bc35d1185d4d04dad67eaae62babc7e4fb26f, False +0806be89284e8a4ec7ef7f81100a1d2c99a7d90e, False +806eb9ac7906ec48cc6148d6d8288153d025cbba, False +807b4b90e8db3613e508a745cd0b821b780bd8cb, False +817c6cbce53c76fa465303855399f84a7253deb2, False +821a2d741626ea190a2ed794c681a51dbcda48fa, False +823e68a00d4aa37069b991bdf75f2d2cced124a3, False +826f161322f99bac553b0d73843917c7fe677df2, False +827df9acf75c5a4ab0384b7c32c230a61fbcc9f6, False +829bf87b541238bb7579a05d18d4f7f1d1f98af1, False +830e2ed47548d8372294609fe7eeca11fb384b29, False +831e2cff446337af8791038161c7d32d96726b20, False +844be43b1c34647e153e9b1c4054bd59a8bb64ed, False +846aba736b25b51bf2036986d36451a37cd7431e, False +848c2cc9428329445b3de7dc982469c583b16c1d, False +848db8687535b11171facb505fad4383385a4ebb, False +864c4aa4b3ec406339b9bc21feff1020b928b138, False +865da0881974291c56a1a12c9acb0a95f664f741, False +866b15a5849b5fa280d8130033062720684aa283, False +881b4930f7df83a2fe6d7278a67c81d4f1f727c3, False +882cb986ea0c0559d606858cf01c8282016ca3b4, False +891d92d36d8ce186ab73959ad164b5fa8d3b3227, False +891f93778cdee4e6fc7133e344bff9f5ccdd1d48, False +899eebd2e800dead618d9df13704e2c94c2f453f, False +917eb4faafe8d1c12b815376f9742eefbe5b8f75, False +925bea10c8bc2648e7e31e696ae81610c194e8e4, False +940d228dddb059ea9fd551bfc7d8ca67e02f20a3, False +960b4f4d8511ffd0daba1219c9f4af1a04cfb950, False +973fedfacd435e90909e96d0877b1013a0a02710, False +975e49af1aa9c80f301a02493fc6643f6897154f, False +979ae06f639a0a37dd07202282f5d65c3cba8425, False +980f777fb6df794f7fab3e644e85c91e879a0758, False +983e167916e12d920189d565ee5a3efb8419dd3b, False +983e239785b276c0a88add59742371176f441392, False +983ef4ef38a08850204a31986545f0e01a8dc0af, False +991a6870411fab1fabc680a306b19bae617b65af, False +993d78f3ab7c7cc8de5f2491d5d04d6720535749, False +997cff30d7278e734d17f9b5fe8033782dbcaa76, False +997fbea849da8787d3831c83d614ca2570479c04, False +1049e1c9bf34ce68e426f4c0fce11b341dae3e89, False +1130f94f33572c3ece77e7e6458d723557469e77, False +1264cc00b9d094c46f1794a56f3c43de92bd85de, False +1295b1a036e8653bd73c59ad75080680c437c932, False +01362ea241206b5668fb03f345c4637fd9386c8a, False +1390bfd3026b4142a2b76e13abbbb550f1137c11, False +1443cd58b03796e757ab8e6abc77861500f81432, False +01495dff5eccd8289fb31fc6df511bf2a043a495, False +1619e2e6a18d5d374963a3a4280f7a6d76356079, False +01841f449f738c1e24fa15753d1fbc5fe0c6a92c, False +2049f5c3e9a4f810916c270f85e639e4326f49ff, False +2056ce1a69f325abd72ae68372a0241ca8d1a0e7, False +2126cc35dc48ccdce854f39acf33a5966046f383, False +2133bf4321d08507b97ce4866d9a2484be485595, False +2155fdf43148aabbd9beb63943df5606fe0c5945, False +2327ae658ac6df2d0c712fba593a13341c4a8942, False +2366c86ee9ee1ec1d3254b392a3a61a249389b12, False +2373e13c60de2cadf4f4d9c62760b9a169e25ef9, False +2641fe1288210d5bfaf624174e89069a0cf0cef1, False +2652a2e75adfb1e0fdcb635c0e24d6bbd8a74ca2, False +2953d685508ac2988d41e5d65b8361c7e1e4330d, False +3033be75f2ac15885e8f4813dc55e168af09e91f, False +3108de6e54bef335ffd071a9016f6b8826497282, False +3238d3a7123feddcf9a8960461bf14e9bb6925c1, False +3250fed9cd01e87e35addd30f213df8fae6ae89f, False +3254d8d6a11b545bf4d63cbeb938fa1585abd353, False +3325d76f019bb4ce6533ec1e985ae3e2d67b693e, False +3412cdbcef645e8b4bd1fd0838ac39d05a93d2a2, False +3432a71596b6cd7e944b6f19cf6d713fe17fc8bd, False +3454e69efed1dfcf54b225bf52c5781d62b42de3, False +3637d32cdd9638a7e430319da9283407b33e2129, False +03639dab61b6903ff319e047b008cb6abec971c6, False +3798a3b8f886f27b7f2d8ebc0c84c6427e0a1ae8, False +3813dd29130016f97c9b7ff621163535d31d9972, False +3820cff3480d571f04ea4fc60c95b3950b8e0936, False +4066fa0fa66bb631771dff6faf8b6c41f677af60, False +4325dd26ec8e6c7a4384a8d6c3f1413b1382265c, False +4375bb2914be29819938e8d7c6d5e5661b296bc3, False +4468c13bc98184bcc403027164ede52b178e5d20, False +4786f8eddaf749039c834efd1ca12ea39517cefd, False +4809bfb8c0ce5c5c876c47cbaffa4b8443e6ec1c, False +5056a49e5f84570b0869f38913c33a89b69e074f, False +5167e9e07073e54f170645e64b4c075ec70a0335, False +5198d5ad511dc6bbdc5891352827d2122f84cce4, False +5234e3e43ed32fc53724bb4052eb1bce39109e02, False +5295cc18c0fd3117b54623ec7c40006e4756a091, False +5339c416ac907c7cb049eb153855816a9fa57ccf, False +5475c0fbc7205d0ddbf7fd3133be34d3aecdc016, False +5508f77968bd392fc4cd8100fa87854abfc4e8a0, False +5522cdf6b9135266e8c73fd545bceaa40f6f7271, False +5602bb9f02e0980f5c2155940a088c7cbe65ea65, False +5663ed3483432046d301e771bbd6b4394b871433, False +5809d4b7a424268172519e54e2ff2b58b461fa2b, False +5811a17461c7679e6285da287fb05f8773cb3f6e, False +5819a4a0b8b640e6e3864c233563c64ab2bf41ad, False +5822ee956db8ac49663ae98abfa7a34c4f725c76, False +5855df4f61c036448115abedf5aa947a6383b1ce, False +5866f3f1fc28a785334cea9f6ceae1373b886203, False +5954c670b64d2ac9a6dcb73295bcb88a212a64f5, False +5990bd240667b753ec647ca7ed0697e58464b7cf, False +5999d188452e209cacb712029af3f3b5b0e30313, False +6027f970cc47e7331043cc22d87f191757557e8f, False +6042abfa29876cc7c46256c26113b50a2837fde9, False +6081fbc49d69b0c9dc9625fc31a33cbef490d98a, False +6219ef05f4a7b56419749e45a45143df8af44495, False +6241c65ea47da414d009f1bb77921dbbad78ca84, False +6342bb30597f40b7df8b207282df821a2ec22ca8, False +6386fffc3938beedfcdec590a25467b4bf74a673, False +6410bde9a047274313c1786f8d28d777aed12fcf, False +6449c323d8a23151b0a1d79924494d143b398326, False +6646f46744931673631d1680de5fa0a334f174c9, False +6685d70c46f4d002d0fc50c70a2f1d577cd173a0, False +06764d11dcec69878cc762c36482be5ef2865443, False +6806bec5437b9907f85780174ad6b8db96650ab1, False +6856a9b774fb1789b383579a369c187d44760c60, False +6913b51c6c46706058379b9f9b3de93c84c33d59, False +6962a0d748b88e71de56e28472adebb9fe6cefe7, False +7025cd14a948f491902ef12724f0fe6e4ce672af, False +7094e30496b2735c235819e99c509769dc30502d, False +7133a5dcd1629d9ad8ab6f52cb94019dd2ba09ca, False +7472af24a8b142dd81dce9d313a6c43ab9b8cafe, False +7542d824416c774ec0e59f22be44b6b72a20bf60, False +7629cab57668d2f1befe0a554e212d4f36849056, False +7743db46f158a33ff13e985f5493e148f0de3681, False +7899d0c5296a70e4f4b8b9c5f3e96ea2491a0ac8, False +08045ba4de0a267910c938a065e0842159d3c0e6, False +8165c556da0db0469632ab42ea5b335b04bd53ea, False +8195c2991d03645f5a989b7aa0601161ad34241f, False +8305fbb4f21343f2f89b3227921d283a12aeb596, False +8321dbf7d07a301d1f3f12ff893cce05ca5ef8a2, False +8415f258d5e129e9bb63cab54c7c207e04c0dfed, False +8651e8d0018be9844a2471273d17157e5684fd80, False +8685af17fa7d4d5f899912196b9bd71d3c56dcba, False +8689fdff97258ee027f88fa5d71b149ad5dfc288, False +8749a9bc494dbd22f77e8b848bce656f8eec2947, False +8834b3facad03e665e1167f32b7c0767c0244730, False +8870a950a73c7791ab6ca03f72cf2609f4e44a3e, False +8886a77527284e0f47d98847f8cdff7984246135, False +08901e442a1995c0b952536f6ab4e2b78d12fc27, False +08931f704e2f2f9aae4d02134dc2f6894631688c, False +8943b22a9d097cfb937008801d0af4a8a3b8ea3e, False +8994a2078b60a17205a34a19f8851ad81980aa97, False +9184b61f915b5f985b67251ebbf0a155e87bd48c, False +9195c52970c032c7fe5536c0348260cb2ae2fb56, False +9316ccc1c31fea6507c2ccabb484bf999c576daa, False +9360a750734fa4597cd593f3309ccc261aafc929, False +9384cf4170fc9d251d191e5caaa514da89c38772, False +9444fd4deeb56fac5538938168e6171d034e0ede, False +9449ef7831c43bc0db23aac79ea442aa71d0db11, False +9468fd5397281258bd0f87c9a646f74181b96131, False +9511c8f5a4a1972fc7d691f6d7a42c87b56a41c5, False +9615ecd3c9de799bfd9330ebd073111aa41f7744, False +9660ea4405f304d30c4610f676185305d0e03c8b, False +09846aa6f8fc996b41075d29111226f326f37e29, False +9847a17d71f678c4d20664542c163fa8cbd03033, False +9849af0395972ff84e40f5c1a51db8a25e3ef6f7, False +10444a1165c1745f960eb6183d24dd05b60e781f, False +11808e4bfc4534caf787fa15ba07bc2cbee95fdd, False +12001de4686bf2e4b9c721c93b35a424dd48249f, False +12289ea8fde534bbaf785b1c800b86a40437717c, False +12824be12d1e4ac567a0f84907db91abdecd6658, False +12951dc7326bf79b0ac170bce28cbd99ac9ccb00, False +14510d4af57832b9517f8f5df9e83d77c7218719, False +14710f3f528e3728a6228b9f56d41076c0d8cabe, False +20386aed9b0a616bfebbf5fbabb283a1d1df936b, False +21558be8922692da15d54962cb7e5ab2c4fb6653, False +24188ef08a9f2783da92954881babfaa6a5bdad7, False +24430dbe765dd54d727d0ba3d234601142805e4b, False +24600d8061ad5eed905e5ed1c8c9455241fad9da, False +24780dc8506e59cb092990573e783a1fcfac4510, False +28098e6540f2f2fc07f7fc6a00edd6ce371a2618, False +28555a5e8afd0f7833d23fb947b228461bd5baf1, False +29760fcf2169f43dc116a4687c869dbe60d3dea1, False +29945c52c4d40f9b31ef806224ad9d98bcd47f5c, False +31637d2699390a9c52ee1cf50bb814d9043b50cb, False +31653c084558e1ecb286adf63a305d914aa3c98b, False +33202e0a2053b838e9df11fade90eb2251c0f509, False +33699eda2d1cf8f3d2dec775637a6c86d795a507, False +35230dedd6b3f035a602f7226fae2dcb61bf1e4a, False +35880f4a551d6b1790f8ded9b23498f1da88575b, False +45260b2b621ba5473292bcfe8995535b336b4244, False +47626eed0d8a571dc80f7bb472d7661a4dc6a030, False +48500af9bd528f8379165b2d2f16a24316e7f1cc, False +49619a8343f4e235202be2aea9a85917d03583d2, False +51572b3463c68617b140d1bd66681f29229193f4, False +52247b2b696aa99d97893c29cb9917ec691fc9c4, False +54506cd1d15574514e7e98c8d90ee66938cde48f, False +56366c90a4519ed83719b30cd4757008f0e558fb, False +56992c68d20498de6e888ca811f2507f0c6707b7, False +57209e65bae481b5a9fa63c4836c33d627739c82, False +57966e0a857591dab70815223ad393ffb6134f5b, False +61007b6969cbb58fbff8f8af738c834c40e67de0, False +61569bcef294f7cffb9ad1a8d99ac6bfee565aa1, False +64070ceff1d42c909465e51ea0ae4c2be6469fba, False +64710c62661a06f2bd68d49e5da3ffe9e81c6960, False +66842cbebd452355f633c318116aeb4ab0258c8e, False +66971c63d2b4e3c6c42ec3563a66db522c2e8b93, False +68185d00b089af2c5eb6392fadb04c6275a676f9, False +68407fbf5c7296351b2c26c2e59510effc87a637, False +69340d8678701e72f39cc01890da1b8af3fd603d, False +73079c97e0eb1023190a377a3311f6a0c2d4c9b8, False +74838e97471c7afeef362d0f94963cc54865ae66, False +75139e50e6f0e54e14634cfffe33e4183b1109ad, False +75837d57167448213b092af97562cf08a590a417, False +75981e91eb9a82ca004d344a68f8c34f0522bcef, False +76102a94ca13d9cdaf7d5a77262cddd4df9a806c, False +77205ffb7a84841bf263b9293c99654f816a52cf, False +80711e20cd501dd7551f9c0a5cab5f3029358b9b, False +81017a44e0b7694b93bcf6ca1f6a46bc8fc8077c, False +81187dd83ea97326b9711f82c3a698646b235aeb, False +82728c060f48775287a921c7530606aebaac0448, False +83239fd08bb93552677345a49b276aeb16fb2c61, False +84365ae4e8622f10d9688a63cb9c083154015e40, False +84815c44d0b7569cd2c023b75d6f11d21bdb7505, False +86275a65ca62f6e046ed9ce1080dd150d3bbc76a, False +86503db5b715ed604e238d74f99701ccc418592e, False +89934a7354b8a59b3fe2bbe22ffe7964a0b8db4b, False +91933e7299ab7b02ee58295f5195bb6ea6b3b46e, False +93767fc355a1afbfff79d54a25204069e0543d2b, False +93951e48e6f29a5592dd6e8e2c426427707d6496, False +94551b307867c4780e5f811f82a9bd3b708767b0, False +96160d32939e3ef146bb30d72d232fc6ac0d1493, False +97162e912362bae3a0ca0d385f9dab8e43360108, False +98082ffefeda96e60df9cd906920ef6918a9e874, False +098172d6d9a91a9b773f40fa4279017932fa6adc, False +115400dd7dd9aa26ca595007f18b5a515626554d, False +126706d2722745fadf95487644ac678e49b87cf4, False +152467fbf5b1840b84adcdb59ead3cebee3bd9aa, False +164893a817b66c18cadf1b98ee74c3513bf2efc5, False +168286ceb4f9758dc69789c4d41b5238e0c2c817, False +174110f86e639cdcef0670d5e6de334338cd22fd, False +183616ff4a41ac27ce0b8456acb0837c5135ca76, False +212778c0c265e0db358ad7c8c1fa9a4bcfe41bd7, False +227815cc47246f7af9153206af77ba174b83bf69, False +309338b4b3a49bf4efa1769b71d8840796f6a14f, False +317294cbd71b7a56a3a38f6d5b912a19bf04ed81, False +341094b867eb9d6b86f227a6cc65022a840099e5, False +347414deea3205ff5e0c36c1acad28944fb5f1b2, False +397597cec45fad93300498e52c2f454e966b2491, False +419625a0eca4ea48186fc8ca8dcdaa9f25d9dca6, False +425289a26069c1e7f62cae59970adf1170754094, False +0463153d9be9a8603ecd0e9af7e7763f42ee6c92, False +467413f215daba3f0a88b154b17ccd1fb7ecb48d, False +480836c9b9499eb97a6d9eba8eeff97def546078, False +514731fa03071a52318224f7be986fd25864a47f, False +519023c34015c16c6104a26a29dfb02b64f4059b, False +533480caf51888cd4bbdaee89a65c1aeeb8d804d, False +556733e012ccc023b0fe97b5ff071d9313718672, False +561105c73bf76152a2b32e4f55f80db6a25ac0d4, False +609530b858465d9795ac43ba435fdd5f12d95956, False +652403bb9a0b199ebdebe538a44bc897eab624f6, False +709745fbd3cc41050840793cdf67e73995e27270, False +743129b2201d36c493818831460f561b44237e33, False +748668b4749a7f38fece03dc8b2c439542ba450a, False +762665bc7a958151874c15edfc2711b161376678, False +791168d38a6a81c6fc4b68f13e0ab4cff2d260c4, False +796933d96e20ed5dac6491d424084900fd72d301, False +807955fd4dcb59b67789b24f2e7bc167027c870a, False +880004dfdd8c2f519bc59291b4a72bfa3cd65bea, False +0915788bc145bbb5baa30d86bd6e13bd83a4ba99, False +917402c0f2b14bf4fb2e2f3db5faf395ce3bc8d1, False +965366f3b0f54c504b434fa829ceaec397a6f553, False +970959aaf829480f960e79ec9c5fbeab32a5ac05, False +2573653e620e455e41f5ef35d8e17d1997629159, False +2612061a57b747118a3d04290d89cd1b398fb2fe, False +2643558f6dbe025d988cf3b0d96a14e362d380d4, False +4044949adb6b821464ccd5d36907963ea9f151e4, False +4243219d5d485cbd3d3e6ff92427006b4608571c, False +4403634c49130a1e9d516ba17d62718238e0fb90, False +4561530bfb8b3aeacadf64388d82000b2d2e56d1, False +4756008dca8867b8baf4436cb610339c498faffc, False +4850297ad1adc65d23c944e90edd076db12afbce, False +5107181b6b921ebcf2c334bec44eebb4a6b1e942, False +5596617b6e256dd94a6af0aaf0a70dd0121262f0, False +6216443c02c998a6652a016a5facb25d3ffd4b7f, False +7293186ad0455cbce06c6619ac81a36bd7d7410c, False +7326537f0d974d05bc0f33003fdc1325a56a6b58, False +8090916af54ef2700b78f6a3ed489b4ab21f54a3, False +9183860ca33a94d7f790a4ad7f23e2ebea77db3e, False +9494221acb244f996e96d36178eb6d964c41dd6c, False +9632613e4d36e2ff4f077317f732bdf0fd0ce876, False +14472289b14fcc1132137b9a8f8fe4e1ce47aa2e, False +14532900bdcd0f7a4599249c4a3a8a74c7facb3f, False +24348214a4389ad5c9ad24fe4f5a5d06f118ba98, False +030425595d318ba1c3ed6d7e1ae6c7571966a91c, False +31658039dc38707d5e41e97a44ecc04206988eb0, False +35356739cd0ed57bce029e485ee137fb3061b600, False +36151274ac573d17f1d89a045986d28c24804934, False +44067754e792986cd4f827d28d3776ca78338176, False +49821166f6f44815955aed0168003c6b6e7d6f27, False +56729469c301c24738e425e052d52fa17a98099a, False +65700067e9d72a8118decb2534719322e8520c4d, False +76696232ac6d502f7290654ec5bb7fde4d9eee3d, False +78950987a2f50c2ec7db57d322ade2d14bb4078b, False +79782167a95f8c45fe73ebe98f4e43264cc4e887, False +87001939e6cf1938ad83249e697db73ae66a96e9, False +93845075e1c4e220eea68160d6028072985ccce8, False +147720973c70dcee789e80bfaa4d374ff3bbaf97, False +331805416bff2bc91d12068625853de085443c62, False +389665158df393b0a2ac6fccc69babfd7a17f306, False +404146070e071454f055cfe3be7811fcdcb7ddbd, False +0435460643f982a6b247ee4cd009c9a56eb133cb, False +494398746b964969eb0d25f950c2beae9faecf06, False +502531250c603e992364acdeb7ced91b9c82e3f4, False +587440399e20487296e26481cd5ad3f3767405e3, False +600571905f308b6164679b131bacaec25a2dc69d, False +644494768bb8a108f01e475e9bb0122105c833d2, False +877256603ad3822ee3f5ecd9c55ad2dfe9899ba3, False +3752800362cdc6ebfba94a5a4f45aa730d81cef4, False +6804953904df94d4abdb0776ad6d55c2a5b8aeaa, False +8252339351f85bd4cb470198ef2ad343ca54d689, False +12279170351af560f40ced884359cc539984e111, False +44641367282a6a9616c91439944d4160a0e6f66a, False +46974629818b492bea090937cc4b541bca7e584a, False +68782264219bba144a96c62db919eaa4f2c2d40c, False +77612660714f967922655f97ccc5ce505b557a53, False +82853146797ce9b1ec5fa02d085f6cf816c243f5, False +99390306328a44c0e69e0d2fe6d313d183e59521, False +198459120142c19e59b9d94063326307dba1be5b, False +277958364721b09b0ac36c6ac1c4382f37e406c9, False +3700414065315caa30b22080b93bd4adf355ada3, False +a0ab0f0d7974fcdc0eafd46ab094a5ed9351d26c, False +a0ac4c0735903452539bd61238cf7e95259057dd, False +a0aee66ebcbeac2e2937109356c7463b1252b668, False +a0c0682b2b668d3784b1fbefcbce33d902c095e5, False +a0f01ff926fe404204e9a7914ba299df5e807694, False +a01b0bf94c12425aa537eb467300a5125132e425, False +a2b84191ba83ccf6ba269ce696d89d84211b9971, False +a2c583da10dc5a913c67b9f7c3c167ca18ef50ef, False +a2d18114421991200bb43645a15359859ee9f95d, False +a02fadd93f936a28bfdc192ccca0f6392c584a65, False +a3b413191de85ee9040e8eee653f9af5738ffab2, False +a3e00bd6988f75faadbc9865302e24caa9202421, False +a3e35efa170d8d4f312116cf6a52c618ddd2af16, False +a3f35118d1faa00f8da7ea78e9c96751a0f5eb3a, False +a4b8d2170a65622aec4cc922103990b1c7783a46, False +a4c301557182738d496fd39c236d6b51100b8c91, False +a4eb380b5fad3422e901e3f864db70ec153f07f0, False +a4fe17ac1cceb3d8c6cf020b14522c345f526d8a, False +a5ceb39dcca171665cdf5b5afe8791c6298eb9e7, False +a5e7d10d32aedfdd4b8118801a537f00807a3b4a, False +a5faa788a66067bdb536364b705735ba7c5547af, False +a6d4ca2a63e1bd0bff603d1fd57e3e321d16587f, False +a6d99ae2eedd4c2df68cb43a19b6b4ee46afef34, False +a6fe309aba3ce8c80f70534ee839edad826661d1, False +a7a07ce28b9dd3276bc1b4a8a03e1beabf993cfd, False +a7a9b8d2ae756986014a94885ed86da2ba797214, False +a7a156058dab3cb8727e075a010473640b3a7155, False +a7d193c74421cdb99350f3714bfc708bcadffd07, False +a7fe66200215e75043b9ff2894b92218e81b9228, False +a8a92bc066a2719ceb9985e7e5996097ed339561, False +a8b7d5b8711b510f744ea07a9fcc3063e98b11a2, False +a8c7b544e50ac5d032cdd676ae83d5e15cb9cf6f, False +a9b6fe8bba412e58780758d98c9e60bb434021dc, False +a9be04a84ab7ec9e041c9eb767281ec87b7096a4, False +a9c23e2a70d19ca93aefc0b7530ed62d23d5df4a, False +a11ca43c77d2df1282d72fa1831298c544d6f82b, False +a19b47699883e5841599452274d671a1bb2c775e, False +a20c3592b8233194d54cd75d882f86cfe84edcd1, False +a23c0acfc3cca5c709dccdf65564628e76ab4498, False +a32ed32cc5693cb1ccd4792b7c3f0340cc8eda91, False +a34eb58d8e8c6c14548c4d3b915eed8b124dbd63, False +a35f943f68faf2b032307f6b336be2d948b400a6, False +a42fa5c6860d9d8e43df42fda817479ab0b31951, False +a46ed979bc467c63532515b763cb72495931c8a3, False +a50a4618218d207822655304b02711b63bd4e2f4, False +a72d7403a0ea8133a89bff0bfc266d443613420a, False +a76b8acbfe647143f0666526ae649fb8099c5c7a, False +a77e6006efcacc637e1c2a49e72232ee0f435e35, False +a78ec8a57553d34deda7f6856f2bc51bc33f7948, False +a87ae8e3bd9b8c3c45d3330cbac583baa957c3bb, False +a88c710a0b90706398a0fd7a9d73123338d04354, False +a093a5341f0930a9fe1f4e56f337f21e40dbdc21, False +a93cadff07a074a9bd121bf6ef22e74572d08fd6, False +a154cfba5371d13db69006e7206d5aef663f85e1, False +a194a188dfcd8e3796529c0263448ba047ce632f, False +a224d4d90820c74bcb18d81bb7020348da409efb, False +a263af6c0b172782f05372c2d5a613cee759a100, False +a325d8b913b7df4a34c1068286532923be31295e, False +a354f2a1205878521964e4eb5fc968981549c6fe, False +a385fc134ef880ca0773e705e0b399da83a950b6, False +a396dd2cc5c9597ede9ff2f6c6310cb65e9f53d0, False +a630fbac79cdc164c9344a588f72d207b6d25e33, False +a655b5bd1b38850ccbda730b900e9ce041419f98, False +a789f832710854534fd892d04ce030f7a1a9320e, False +a0800bdac932b41d0e661a1dff7b03fa44b0a4b7, False +a858cba39573583f6aff0a31d237bcebabaaf503, False +a919a3e6b458e57f566b18142d568e213911d825, False +a988bc50b88f4e627a0860ff647e1c9f62ea66a2, False +a2135a6cf11a93b6b74d5607a2aff7bd0244f866, False +a2546f59391df58427404b45a53dbfbdc70f77bd, False +a3044ff89c23c967ba9837c4f56007675575dbd0, False +a4990cf91bd66fe1fda09a1d5df0d6131180939c, False +a8779dcb4d51627b49efb7b58c281e0ea42f9a00, False +a9658a8630ce5df1d724dc47334c0dc093a880ed, False +a9967a984dbe294a7b789d969c5f1ed6a9dbca5c, False +a65297dccfaad2014cb1dfeee9e07ebc3dab25db, False +a99726b1fb773a189df763d7588d4c5a34726f35, False +a279753d53f40ac43e222bf0cc86b08ce0911d50, False +a711469ae87945b914c78d955b0919a033603d24, False +a924325b9c670162a99f983687e3d14cc35a5302, False +a3028126eb2b6dea68d6dc03753e3e04b1552f26, False +a443254659cf685e65190747772d47086f49d81a, False +a632498169966d7fb799ac049da4ac322248012e, False +aa9f2c686d690912a117f12ca4928f759bbcccf3, False +aa48e0adc7ea84c3c8dbb9530d5a56be5c22c13f, False +aa69ad95160221cb20a99652a2d8b905bcefdac4, False +aad01c69d7a27a1740e422f5f64b781816bd86fa, False +aadfa2e05d0c631a482bf15f3db20f6da58d14e2, False +aae86871741a96090f4a07f39db579f778d53aa0, False +aaecdb19e67392d21115834f481a4c7a2660e90b, False +ab0e61c699e14e1d9f691c93a1ad0fd2cd7f931d, False +ab4d4cec0a502c7fab16b6a7536e638c9ac0cb6a, False +ab9ca298eb045d3a2fa543fcd3c5436273d146b2, False +ab56aaa7f6e5e1baf9382b507d105db411484a50, False +ab878c08d831f453b8a7ec3d817afcd6610e2423, False +ab7605f386deb7cfe8f5251e7897979a06b4b879, False +ab147892f0d3e77a2ff2cf511c8cd62cafb2b973, False +abfe6b503ca50f702989ec29e758d6c92b65bb5e, False +ac49fcf122c3a29fdce60f17e669ae23605561c5, False +ac0817d0fe557345c16534a7c1f25ae6e2972393, False +acfe3955254d50d2134fa4ff31493b7d3f83cb0e, False +ad4b7198f5d3fa326398c1c239a4e6c830ed3320, False +ad5d977baca487584cd1e79e6289039bfb019fb2, False +adbfdd15598bbeb5e426dac204cef0b4ffc68afe, False +adc2744061c0483d98c1a5121309cb600dd1efd0, False +adedadfe55bc64acd002257edc522c8ffd9a5c98, False +adf1a6430989bad369414f30e27f74084f3d2cae, False +ae0c378302f7fe60bae22dfd25ad9f2713a08505, False +ae2c7bce156a488d4868a8f494f6050fad5582c1, False +ae5bbeab569aed333ee50e683afdbd4d96ec93a6, False +ae9ac13b579222d6f9a3459151e4562892b1aae8, False +ae09ba6c03141b495edc4ad1d07ba5fc00b78c4d, False +ae6357464d4b54f30ffcf46747d34ce90ccd075c, False +aec9ab12b9fed5f9d8fe18ae46e543a849a4b05d, False +aeebbe82b3781a41a9e0227feae371ff8497e72b, False +af3a929bc505b1f6592c4f0dad73cdab191fb20c, False +af45703d9d6e1e327161f253cb6bde9dd479c232, False +afa04171fbf30e2a5e5cf439d4ae70cdb7a88c62, False +afc34f614d37dd15c323fbdb9df5e67023c5bc84, False +b0a6e1e1f6e8c6603caba52bfa8e3e06469e9eb7, False +b00a45d53ade31ca46d432f98426b88b0ac552c8, False +b0c0193881ab6e75e9d16e983d21e7c925178d2d, False +b0dac93072ce252e3cfbc1ee7bdf4670478ac76b, False +b00f7a0bea75c3a7834d0c4397945c2bbe6e6381, False +b1c0fc607c063010c4b9300c955a0e3e5f7001fc, False +b2a15fbf1be55416b27bc7ef9a10d2b22af24376, False +b02c891d8e86bc914852753f1ce1c318b2499f42, False +b2d2f9ca104169f9f8b874be2a35b40c77642439, False +b2e04eb723a364eb03bdd4a3d12cdb7d1ea1f6c6, False +b3b7780480e764c4178da48937126a9e4c001627, False +b3d4752bce3f27d455d3744e87d3df6b192bbba0, False +b3ff061d591f5cdcc087056fb3ef0cf6a1ce971a, False +b04c289889696c7f1940681de877c1eb9d5b9edf, False +b4d2dc4541ca7302ab8356fcd7a623e022df7888, False +b5a23e82cd0af7f35dac26da79ff5e34d4efdd2b, False +b5ac5dd1dc346152f1a07d3e40e665de05095852, False +b5b29465833cd37f1b2c33b6853787e0cb72ff3c, False +b5e518e3fea5db109669295a3b91d6e2654c9ae6, False +b6adef6df83aee1043abc57cd78657ff8ba6a7ec, False +b6b8be444422c4eb84654d787c342f6f29f28842, False +b6b31d28bf3a2d2246c08b31c7943507babdfb8f, False +b7be6ad871800511e7a320626aac37272eb919c4, False +b7cbd0dfb550c5c53d1fd17401018d8b0aa4a216, False +b7cdc363ac4a1beaa2872daa374e24013ca15338, False +b7fcaf9b0e97c16c5db7a507f9929ef8599a2113, False +b8bdd9cbc1ca695a206583afdc26f1a4c3987303, False +b9ab4cf85f96a453ae2c668ed5b9e0cac8025827, False +b9acee2dff54f19f51dd086dffb1db280cbc6b25, False +b9be70b3bee020cdb0a2406b92ddcd88c2c2b1a4, False +b9bf8389a25eecf33dccdcc60589943e94798e51, False +b11af4142176ca22da20219cecdbf63bff9db8ce, False +b15e1d3e55fa2b36c327471ac84c3b42b562aa1e, False +b23e528013a3892d372731a5dd2f13da2e44bc97, False +b035d7f8ef2f8becc5585d131ad31c5ffc708083, False +b35edd9663c76d8dce5189ca04cb4b44d9d009de, False +b44f832966b52e31a8c23f1ddb8175b117d29c10, False +b52a045d6d0dc10cdc872ffc4a932a3d16f4e6e2, False +b67fbb54b56e22cce0aef0d46f5d52e41735cdf9, False +b83ed34420fefdf8063b9b33d4c1305aa534e75a, False +b90ec083667e82b541010a09146637f9f7da3698, False +b97c21bae5cc1a4a00428838eadc96c22c76a5ad, False +b118c32fe2518b918e695a488b5d22fbd1563460, False +b136d105af1f63f9caaae98c3942a9415123fd0c, False +b0353f3771acd6fec27b80e5bc92196e4d0bc49e, False +b556eeb736ae52a83f4b36a03fd6a03c9f941bc2, False +b578f8dcf387e3a7d843f9d98c69d3ba595d013d, False +b579dad917d549c04323882cc1c55f888b0253d2, False +b603d314f93de8bf2a5aaf7821bdafbce1a2c8c1, False +b0653bf54cea7fcfb1cde2dd0a34da92729c3a2e, False +b672c86af96ff069503760f15485f4f6f7174eb2, False +b759f0cf80e62d9701b18edb6791d2172cf11c36, False +b0855c905d4db001d30d8445f52542d199cb3453, False +b897d91c40a2cdf637fb76a72f0799e9440ecd75, False +b927ec201f8a3c6cbc72f3a6077fc0d1ca12b0a0, False +b2414f3ff69c153c9c9b0095108c7277fd1eadb5, False +b2732ef497c4e06c0cdc2b15b85564008c2bb9ca, False +b3839ea63c12168f51635e0f01f431fa4de910b4, False +b5721b76419b4315313005ce0e2b66d8aa714346, False +b011192f42134fc5005b59561dd77d99728ddc5f, False +b13253a6054a680cba603ed5d4c12f081d38b098, False +b16142c1f913731acf684391604959b2989ff917, False +b87936a4a52e2a0b805369a0763a536c719ca7d6, False +b89676c68f7f6513c6b3fe07d46350e427acd54b, False +b367771b1ea159b0fe7f536f8dfb59de9f317e91, False +b0455337bd2ea887aef88a629c96c7010d20a6f9, False +b993094e6a2356a77f29a6a74e88874463264ae8, False +b5729920dcc9568aa10fa3034e37566ae8944203, False +b6180942d5bbb22c065e4f7f2bb39f05ca21f251, False +b6645423faac4d8a1bfeff2d5106285dba2a8a00, False +b48729416ed78da505af25fd953a52babdaa421b, False +b255272167a573b0dad1637c5c250e6560effe74, False +b460642155f053278bd1ff6d057e6bcf49ae8ba5, False +b5813425898277762224ac7c5772d3637586abfa, False +ba9b22943a7967908955980f309420e15b3d10e2, False +ba231dd136e3bc77fb04ade17235b923aa7b2f07, False +ba726765d25fd53f9c3e2dab6d63802d3201e249, False +baa2fcb64c8c07e7f12d3d1b57f893b232942509, False +baf5adfdb933cc6ea9a8e3dd24b64ff6691cd5a4, False +bb3d4d2bb300b0ac4a99aa0b377123de497bea3f, False +bb3f594366eccfe9dec0e81311389b85088299c7, False +bb3fe90bafdd46b77abcc1bfb5773d1cd110b8dc, False +bb14a7d251985c9cfc8a8488c24949ffca4ac619, False +bb034d375833247e8539a25b7075afb01e908507, False +bb730ede5c3c6ccaae4f652308fea653d91eeadb, False +bb966a4f853df29b8b37b89e157aed4eb3936aec, False +bbbd1200db14a6d56f7e0ec9b761c4cd011794fc, False +bbc59bc49585798e7a16d692d5f44addd59e77fc, False +bc3c5c45d5a9126bff85470c7c48d4e2b7ebfd0d, False +bc4b900f79626b0647e8767f126f15094fdc500f, False +bc4f83ffe12ce0b34314e3fea983c360d2ebdd5e, False +bc7a345a9caebcc5068af441892819603f03d74e, False +bc7d5693a9e657f3127802a9632863853bd37497, False +bc59a49ebf99164d5ed88bd6eaff12ec4ed86d0a, False +bc87d100f3cc07ee8bc157fd35314bc1de216984, False +bca3e0bf764ec84a210740f33a03678a66c183ed, False +bcd4cee8acfd838f9b678d15286ecc841f174448, False +bcd9d78bae4933655351e9d9596286e43f3f516d, False +bd3b4be2f8e07b17bc87090468aca049b1e3d636, False +bd4d4877984d271dc122d924b4c733897095bfc8, False +bd6cf3d6b2264461bc7cdd4efacd5d80b35bf8af, False +bd7afc0af95def79ba0a85559ba1a35c9b32b83b, False +bd8f1a9eb25a71785663f03fd8e8770068ea49bb, False +bd29ebd8934cde447c1c219e180cbf3c26831c7a, False +bd66ae3b3a35cd718f3befa8d2dc74205d537c74, False +bd60660df8e8d48fa3f524a7536d1f78e6e0e21f, False +bdab4ca579230f7652590bc19fa00a35701857ef, False +bdafc134880c09c2f9415807dbf2a680683a6d06, False +bde8da056a43d2fc9470645a35c7111868be6c50, False +bdfbde6e190d9e7ed0299c2e9f14693d7bc62e85, False +be0ea104f86eedb2424627de3e52a32af8d19c02, False +be1b0ef31886b4b2b42cf0bf1b7df548917c9943, False +be4b4e66accf200a921df7e0277e184e0d9727b9, False +be76e60c5b196c712dc958ae97874f28f4817fff, False +be354ca7f1b4e4ff79b3bf0097152a73605881b2, False +be948ce6d040db95c6cfa9e333cd26c619e35da5, False +be022989c87bfdd16986c388daab10c6780181bd, False +be25821fc61cfdbefa4e687b3d0e9dba3ae26903, False +bec5a2e6143d4e219f4b2db481a10bb86f669e7d, False +beec92d1124adb179e9ac35d3955061c93424367, False +bf0ea53947cff924371c0ae4abf8b9891452e9c5, False +bf02cf6cbdba130dffd8bd550a2bb2eed8573c62, False +bf2dfa9d2106781b50ab85f7d875aaea367061f9, False +bf6df51f2869002466d2745d7565b18627d397b6, False +bf6fcec03636f6d01a306b806e89cf74e80df7d2, False +bf10c8dfedc7eec580f1529066c4a1a4caa442a6, False +bf330e41e4f3bac3abcf00e44781b24d76b8ea13, False +bf840db863fc9c2646b2f8f372e4847b2fd42e34, False +bf796005b2234568661e5c595ef556a1fe472907, False +bfd6cf98dd195c4c70cfa0510fd4eaed99efea6a, False +bfd8c5a51d30f073bfa7daa2e4c5c10914103ed6, False +bfe04e96e993785f20979427194d364f5ca00510, False +c0ee4ef13d4cc4bc8f549cb9a1fd8b1f213e7d18, False +c00f4c9ef1ca59a2bccb90f098e8a11f2fe80fd4, False +c0f56658f8f0c75ad34510d3a5eb7686c0af1fa8, False +c1b60402ed182b1b5e7187b14215797d6f30b7e7, False +c1bdae17057dfa88d5e3894433642030fd66c7d6, False +c1eb50298ff734470f53763d12f8bebde8d61764, False +c1ef6d7dc8b53ca1b3e96457ec62a5144074300f, False +c2a02097a8366892be928f0485471d898002276b, False +c2ad39c5fadc51ab84f17820b783af71a3cf8f68, False +c2b963967c1e817256ce9f72f7e160a9632947bb, False +c2cbb4677994a6d2fbd4796360fe7538d9189eda, False +c2d198ebe2cd8c70b0886e60d98b327f1934aa0e, False +c2d966559ee73ca0f88779a2de90d8158f85e4fb, False +c2e8cedabb341d4b9966b4812328ea09645b06c1, False +c3a726217d3a5aea52bebf82925a734ef322e529, False +c3b2adbd3b89bdcd01f1e813bc4d2e06975ec727, False +c3c743ba30c2c5488903064178e52a7b61297dc0, False +c3ccb52aabc1a2543dee20239c88cd307340c134, False +c3fdb13cd96d001010a51bf5cacd47503c75df7e, False +c4dde9b2280f0318ce9b59e388883bcb101d6654, False +c5fe7eaaa71f5cc7e2bc468d56bbdd3685b24224, False +c5ff5698a48415c704891bf4b3356c31ac773721, False +c6e2268863e1ad826feed40bfee065582fd5f0b2, False +c6ee2a801e720442092ed6497935cc067158e761, False +c6f1458e0bcabb5ed4ff8ad1086d192bd04997e1, False +c6ffbbcdb60a0cf942ce85663aa54754cd760551, False +c7a970b36a90760399672531f4a75e0ffe9ba35e, False +c7c5a1b22bc45672ee746040a0113a80f8e85531, False +c08f9ea242376a3efad0f8002b6e707c68acbd99, False +c20a0e84afd93a3008f0df44bb00a7fc65913557, False +c23e159071acb3f9bd03ed01576d9077cc30f1cf, False +c32d5dd6751249c2517774929590e50a01d56a6c, False +c32fb94bb53f10fb230103eaaf503752000405fa, False +c34d55162d1b20d077c536fbafd0a1430680222a, False +c35b7327609100ad9504c9f4a73c745fd1185c0e, False +c037f556753b66e786c130526e4b807a8636cf1b, False +c40df2998fcea3687deebb651759eb24a23e5922, False +c41e7d7d088f1794f6e78781f6698faf445f5c83, False +c48c42dccc86b2f952e52a76e46ae4cdf7744a18, False +c58f80fee5a4b2d9463effc9618e33b68eb7a9c5, False +c59d292da3e13710b163afb8324ab7141a6cfb61, False +c61e8e4d12958dd3489d22fa51c47f14622034e2, False +c70ef2f552fd561ae25806cfe63d5d7e07ac6f76, False +c71ff97be911ee5e2438bf3ddcf15350942dbad7, False +c072d770784b824c576201ad88a3c8c9e82773a4, False +c73f8b5dd9353b05bdc81872432e0851cc2d6ab5, False +c76cf29427780abebb11688b4815cbe59d54819a, False +c77e8ffcb85d5d91694bfe37516ef9880890716c, False +c85cb53d51c50dedb4d0063ce0f4a168890352b3, False +c91a14013488c357ffc009345a8dcdbf0cbc2aa6, False +c95d0ac8c043493a0b0c13f11d3b83d12c0a24f9, False +c96d9c01eeaef13563d832710c5717cd77d78648, False +c97d8bae2764d5776100de73c8d88af8131fcafb, False +c192b6182392e8ab3704f8f95f8899fa0b6c7cf7, False +c204d5b4284b03f3418de66c856e7c64ba74024d, False +c294ee4dd27985e94399a647c6431420ad3749d9, False +c498d4ff8a6506991025a1a036943ad3f82c62ac, False +c547e4f30bb766fbc780ad43e058efcb183f53ff, False +c573a0b830810d34c543f2f530dc5572b22bfcfd, False +c591c70a1493babeaa31a1465eb356201a44b4ce, False +c677a8bc97aee16aac025979f5c14a953859b9bb, False +c680b3d7f7c7f4415ba590e80c71a0a272ae5480, False +c700ab9d2434c38355eb1318ee868040efbc4bf4, False +c788d5a6d5cfe39a4ab5d8e33f1e4e5c8e8d3652, False +c1230b54c3b732617601d2413eeb180c5d29f2e2, False +c4851ca365614ca521cf0f1418ae2fdeb8480d0e, False +c6727c642c3dd018f4ae964f7ed8fccf95a0e204, False +c8066b282efeea5f6d7930a742cb57ffdec6105b, False +c8867a24eb06b87f5cbca53b65b290cf5ae57b67, False +c44931cecf8bc5a93911345b82669fe2ed60acf8, False +c081738fd50bd67e91cd3f9aa6368721985b9e40, False +c95829d613a9a780c467c06f0402984dbbc8db06, False +c577041acb239ce222181e716bcc5e3b4c097305, False +c4633312e799ff34f1fefafb24e5011b18d43171, False +c493273348e661dce9c1690dc54fc625ab9cf834, False +c07383091478a013cc6f7542f6b5fc8fbf520a6c, False +c016370102275e55cb6ecabfe4b20dd1ed4a230e, False +ca2e3f978e57f15629649965fe52e0ea962998cf, False +ca8c1111a38843f3d2d1818f2e04217a11e97884, False +ca48b12c9b33f1c72d3af87e064f61b7cd197c31, False +ca241f59f18a484bb401df1ee05f2b1ff2fdf24c, False +cab3167deba4d96270450d0365eb3e8266d3a79b, False +cab394212d39fd2760c820003ea5dd7511e4727e, False +cab463553928979537ae6c6097cc5d0ecca9d802, False +cac04279e6d51203058f8f64ebad61e4f4a376dd, False +cacca75ff25d59f01deba6bac2d55bb28b9651ea, False +cae4c60830bba615ff533dc23ffee6e6e5c7d14e, False +cb3dd8f2c8de396606e0794f6effc921aff7235d, False +cb73b8d7a7eba06c5070a71263b39705724e9f7b, False +cba2876e0d64126946169da1d9ed044e48f9e232, False +cbb4633c95d4b985f10188b8d62be4a90593b07d, False +cbdc498e2afe7d309231b96cd7cbad94a0aa3f35, False +cc15f4f67e55963a009abe0f4fe10148cb632f2f, False +cc34d2bd7d33c5dd311fae8268857ee35dcbab47, False +cc85c356e547d86975d71f5405415e1019706178, False +cc90affb5036b91b64564a50865593eedad00e75, False +cc591e0bfc275461ad38bf335cf940a3e47ac0f5, False +cc57668f1affa1e9b866ce5af34ee20c7e7a0649, False +ccb76c83b981fc6f627b66ba20f3013ab866c47f, False +ccd92dda462cb380a92851ccfe000079bc3c82c8, False +ccf3f0ee76dd2263f77ad90a7cec59da84e047c8, False +cd2e333e291c64ac4acec135ff84956ecabd875d, False +cd4ffae2b879d72de5eb67e2876f7c3fd7a3a740, False +cd5e1e78e6d0ffda3ce78803940b2018c1f37c84, False +cd6cfef40870510516a4cdc79c3168924284cbec, False +cd8fa11a328a4c6a94f20a5c2da592ebb29b3729, False +cd17a7b8d78ee79bc52015841577a2652f9e0625, False +cd196cef130b539fc5f2596f6acf43384a63a515, False +cd69767e95dec46a9ced153cb9faa4e36d595805, False +cd9958285c01f3c54618961e744b57b2c4b90cbc, False +cdcf14da5274eb889d420672ab4b49de3ee631ce, False +cdd7e787c588639f7f7418656396df152f8a2a81, False +ce7750010940c4065e644b0f8dcdbbf8801f1dca, False +cecab63706032928094ec5b80cde48d9f61e2ca4, False +cece0c7af35159dbcb8c33a084d522d585a8ec3c, False +ced8302d0499c4e88074123d385c00cb7074f2a0, False +ced51760e36e33d31c5a793d326597c322213c53, False +cee1e41a478c3ea7e733d3d2f6207bfdd1b54af8, False +cf0a26f26d1513d6a4300bb2d71d958122ff45ff, False +cf2ccf35f30d7f7a463a0ef5eefc18a90ea7d2fe, False +cf7c885e0df1ee2f9480956522ed5c0ce1be1cf5, False +cf8fa382afa5b18e36be4b3987dbd473877ad977, False +cf9fb707f111d08c80f8d4aa84793567ce13daf3, False +cf12af67b32e99b6409cb812b29375631ba7d7cf, False +cf628cbc7e1faf1fb02bbfacf52dc5ab9f71a9e9, False +cf5190e7f1fc590262ec33b64c3570caf0d072fb, False +cfa828eda507f07bda0a4ae48dad583b9f6f4c07, False +cfaa0aae3d38e8a5398763d3a6f9bd2342431d14, False +cfba3ddc51c5c21b0ec4e4ec63fb2d28e34c2ffe, False +cfc85f51a78abe00a2b5a068447574ffb4c010a6, False +cfeb7e530ff927219de3ec37515bddf33f113755, False +d0a90cf190c1fc60f0446c1855ab5e4dcfa51125, False +d0ac83197c815743638fdeca4530127d8f20d4c0, False +d00bc6346c6372ca3b2daf6d6fbd181c1225510c, False +d0e9a70969310ca5b09c46606d1c8959edba16f5, False +d0f73838a7772e446c6952ae69dcb11779e1928f, False +d1bb1e76ecd549767fd650aa211e3ce29be75ad6, False +d1d1e0cdaba797ee70882e63f66055675c3f1e7f, False +d1d3542887f4bc9da14060afc1e332c155a0cd48, False +d2ae5957360187e875bf79838dfeb31f2d736128, False +d2d6e99d58f28f41b8c1208013009ae3c02f8a3f, False +d2d44412052fb3c3548694ef5ddbc1433704ceb4, False +d2f7d40d8e1a56ede42103027842e000d9cacd3e, False +d3a926adf018e827f70d37a5d2997c825d9bb5cf, False +d3b1439bf4a338a6df4e4421c2186835667fac5d, False +d3cd758b209f9b187fa3bc9d99c486ba0f3903bc, False +d3d9a98182829d6501e0ee32e51505763063a80c, False +d3d8114665327c3331db90d3e406f0e7573a0a83, False +d3fd1b00ebe22586ca981e3107a0b1f70d6d41c2, False +d04a9639ef6144f44945d0f4e783cac521d8e3cc, False +d4ba31e433421a8ef008208e9b868da12e24a5a5, False +d4e0d78439767eedfbf0a3ab83301d472c8c13cf, False +d4f54f53d983c87d9b36a64c5e5ab98a899f4280, False +d5ac460b0fb98c00928d2ea17fbf6afbd33d6da0, False +d5ba22e5c7328d0791fffe08a5db43203a490ebd, False +d5ba163ba97f94c7aa4a4a625eb0547b8894e1a5, False +d5c83369505d1bad9bcf54734a507870124103fc, False +d5d3d6b8376d50b96e2c1f4333847dcbd4724b59, False +d5ea46aa783d6437ce4c55467bc780bce615b252, False +d6a4bbcc213289c3d7b48e7dd6ab0d376319ddb6, False +d6a8ea7841967baf3eb2dcb20eff191c0382c9cb, False +d06c4e7cdfc08578e0d7f16725f4453c93a401ce, False +d6c09c2eef40f4a2b96cb8dcc6ba9218bc54f393, False +d6e625531765561676efdaf7c8ac013cf59d7eb9, False +d6f84b02f76e72be86c7e9b374c6781752270735, False +d7b8594541014144aca68b4bf28d747263b549e4, False +d7cec0e53dadbc4291064708c84e3614b79ac3c9, False +d8bf69588c8da9284c2d02ab812c0c36d2c6340b, False +d9d337b83d72bfea7f8ca950b1bed4a77fd872ec, False +d13be6893c60fabc31729e397e85952d552d3d55, False +d015e61bece2fc81cddeadd8bd55d97005846874, False +d17e5570249b7ddb8e6bd201144d66f128db8a65, False +d19eba5ce4f2e7ca6c3166312c236f1297fa2590, False +d34aca2483aff11d473b2d8f23a3e6a9fbc1ddf9, False +d39d83b10a9034353d5b5fdb3d39dde23aef98a6, False +d42d4f924fafaf8c326cca055aa6f21b0852d32f, False +d43f80e671e1dbc1ab4292cbc01334c4cf61fe30, False +d45e6fea873ce5afed4d0f3f4b40399451499343, False +d46c006fe0b98a335dd2097088af74817824cf3c, False +d46fe2e22344617cbd13f93c50a30a761a27779a, False +d61b10008e1a0fa9af639836eaf9b6e63be86609, False +d71af85f2f9dbc732b08fe4fa3291df1f9a8df44, False +d71e6b935fc49a8cdc391283eea037016956b6b8, False +d78cf4bcb711be4df5d07b1813f18bdc2721be6d, False +d80b960ac5f15a404c90b594a716fc78e550cb4b, False +d80f27979b4199054d9fb8becceff6d3a383d16c, False +d94ca9c2481d2a92c27a4cfa6c31969e9fd2b56b, False +d96b2b9537c7c721d5a79b375aefbfacecd04f65, False +d97c7ae5ab600872266f89c8a5b375ce72ecf3f9, False +d161f89dbda03dc3ff807fd144e448fa6bbc32ca, False +d213d8ab611456a8c8d1f8b1f76bd075d9070f48, False +d445bb243f12ba89fc91c9d2265408b5cd635632, False +d463ac21f74cef85dd8bb9ddb81d710d461f9efd, False +d502ba2ae3d3aa9ba8acb4fefc8ccb030915f77c, False +d595ed74fb59b637c5175d3d775421788ab9cfed, False +d599a7a59c32fa7cbba3ffa320de452a331c131e, False +d621c3a39d9291f0943531204d68e705633986c9, False +d844cf11bdaa1246053d5e83170fdea4dd750f0d, False +d869cd12e539239280c22b82c7a0d0136c0f80c1, False +d913eb66da3fa49ca0690dbf057279c4a05ccbfa, False +d983d1c179f32bdb1edb214c941f8b33079fa410, False +d8814a0e0e3935fa2b36aa1af3e5f968d595a49b, False +d15217ebbc7c951edfdc9da15dfa5fb5d9ebc12a, False +d35907d5ba40cef9835aea3cbb808e9a92d9cbaa, False +d47861b542c4f2e8fe9adcf86d55e26e12d1a213, False +d52372a1fd345dda86ea80c9ae5672af49ff98dd, False +d74335d420e30b73fe0f8859cef6e5701b0d0360, False +d484180bf96ca1ccefc343489d9c687682730622, False +d898715b817ee6c34958abada9ca65d8a40439e5, False +d3064870d4e8d00b37a7acd77add1b45afda7e28, False +d15420918c690dbe0c1e62db3e69a94db6ad109a, False +d29068621d79dfd804706cec522734681a9866f8, False +d34539104b7eb3776fa7e3259c2b5ba6e21154a1, False +d52301544c422bcf636ba41e4b108d50998d7d6f, False +d44836372348cee4bb44eefe9cdc2459f78e0ca1, False +d6343889675045754ee1964dbb1e6c88f8ae5b08, False +da6ab8a359f639f9bbd92f51bf7ffc89f65714ee, False +da7f5e58294c0b934e1bccaa03f3694275bc9ffc, False +da79e0bf2e7f7c777b226234b982498a964f7f90, False +da2157ae8ca4a7b67ee6a45988fda2628f9db124, False +da5808d15d8c00f8b358672b2279c5c3dc1f22f4, False +da76633c400abd9f926df655f907666e2119b280, False +daa2bae8cc5b6febde6578bf827b310107619caa, False +dab955e709dda5c446bfba2e7a8e4b2107185fd9, False +dac7d6d936614f7411e0a00d4388c60871db23d6, False +dac9f126f273e5fcd4d9bb25b3aa96cd623aa59f, False +dae79e07aa34f02cd3d9a626f730664e8ff1dc43, False +daf22e426300f5b1676128b22affd6a5cb40a285, False +db19f85684c964132250926f988da86ac513baa6, False +db064ef059f7bb0af9f1036e7649e4835a9ff4f5, False +db74ce14813eb0d5c54c8992806b22fb846ee34c, False +db99ea06891a3dbce721816075f0f8de683dbde1, False +db360d1335005af5d247884ecb78fa6977addc6a, False +db592834ff9c6f21b06f6285319b5c5b1596afd1, False +dbf827cab332d1b993e1f3383ca84108f70b52d1, False +dbfd7518092c6d02b2652765b8d2aaabc66c68cb, False +dc2bf6dbff44a99b087ba5c987bab9ac4e28e53b, False +dc4b6a1b56faf0b0ac679141c9c53f1449bf984a, False +dc7a4ef39408cdfc7e9dce48757bde5cbe4ee2c1, False +dc8fc86daedb4f8cf3b4468d1f9b7c8078feea43, False +dcbea81e77e676cd98dba9b8a74d3b52a3e35c1e, False +dccf4f0724add1d24511d9353a2c959487df35e6, False +dcd2233f52efa5c8186ca4a748b17a28256ac703, False +dcd5367db3883b20304a049a5da922136e292edb, False +dce63427a7ab03bf69ae6c448ee7d162de9d26b7, False +dcef0d475b7fdea2530898215feafddac1fe9bcc, False +dd3c29d0d8f9ab311bbec6c04df69730f7d34a4f, False +dd295fef0c5d275a25f80090cbe03d2d729d2b8e, False +dd665def2d6a4b899e40affc18c777799f0d65fd, False +dd743e1b5d46f59890eb5d0bed057b295bbb5f16, False +dd6582ab19f006bd12a8acc846461662735fbc00, False +ddd93a5bcd4c806764adaf90e78e72de7eca4042, False +dde601823da961329b32944b1f12b3aca1535d28, False +de265a1735dcff9dd247bef674803fdc80861c33, False +dea19c6d29caf59c8695380008ab7e79ecab0c43, False +defe405f45ef55bfc876b2d25b30762478d9a819, False +df4db6abfa9250164ee856f137ebae9f34cff325, False +df7b1008225b105b1cf4e67004b5a00e23cc35b8, False +df0294d1d608ecb3d5caa16d901a41acf0215969, False +df442f882b2418f21617712b6fd5989cbd8e54a5, False +df864a06ed866275336fb19a83e536ddac44cf9a, False +df913b62fdd7ff50ba22210b3dd697861dd91a89, False +df43875279fa8ba60adf1cfec4e5fbc7f353525e, False +dfba8399c26d06600f33cdc7dfedcb0f60708b4c, False +dfbad821a292e7522046cf89878b7bb44951d211, False +e0ac49eeb402a569745dc3d1f6261493d66e2be6, False +e0c58f0e9cfa8fffaa6707541a2e7a79754a99be, False +e00cdf595139fa3377cd57037c9a60263d182fa3, False +e0d5b8aea675e79541af41ec6cbd4e132d82cd37, False +e0e66b7c27e7eb0cce22a57b38e42b47c9223760, False +e0e445de0c80e1d1092bede074e36a46c4e4fa61, False +e0ec49fa0d7054afb2f6e2533a72b02f58544eae, False +e1bd0f66a94b27878f1114d2341c3abce3491189, False +e2a82f70229b83e1b270a2535e8dc9d37fce2acf, False +e2ccbc72bf0fd7d7c0ac172153fa5441393463b8, False +e2e482db368c04670761f413a6e5b5dea0f3fcbe, False +e2fc4ce7367b1e8b5a36f3b4fe67b76eef514b82, False +e3ca4616713c8ffa6e9292f63e5c5eea48a06675, False +e3d12cbfbbc6405329d010928c738dee3ce3be67, False +e3eea412d1eee1cbd06a1f59f254e71b9771da1a, False +e4bf06896b67fc56bbded3bbd2086ad6dcdb7746, False +e4eee022d8016b7fb9b3ef2e74a6ad3d61d306dc, False +e5a69ee43da8cb7812bb642595695b98f8165e87, False +e5a3564ec98cab466b427097150536e18bb657e4, False +e5e5164617d57163bf0a422c90ace89bca38a1bf, False +e5f4cd6d3514d7f672ee16c7df0043f650fb911d, False +e7cdc7c7606c32dfa595f61cd24581fa84f4544c, False +e7db99d6a6bce7c5fd40273b538eee3add51884f, False +e7f247969ee88cc3aba4d0534849ff00749701a0, False +e8a59b3cd7bfd22edb9dc6072984e19fb913855b, False +e9a173b62d5e9a6d43eda38068299480cc551ba5, False +e9b2d3685968b69c60f5e058c220412eaffaea1c, False +e9bf0aab8dafe8c727b4eed113d6c7405048f14e, False +e13ea7f9619eb3d062fd34635dcad7b6d500e3af, False +e18ae6c638ec54e7e75a4dc9a1b2a3e81f220d6e, False +e21bdd880d7b2406b24073a9e9097cf825a8c151, False +e30bb09fbbcc21a9047e05f1fe05fe4229907612, False +e38e343fc8704345c2994f67cfbd587f26160367, False +e40eb85bbfeb96330fa43d3a766676fd4bb9fb3e, False +e57c3b4a26b3e66a4970aaa5e2433464b9e37e27, False +e68c2e4d5bd2f132865a29e2a577551a55111947, False +e69b1d450585bdc223ffc8b035d980c5b880c920, False +e71b1c50fc7edb99c67849438d91ad8cbc3a7bb5, False +e78cd5226bde2fd145dbc897fff209bdca7af20c, False +e79e4bec28b46c6aaa6d82f2f43b2652b35fc13a, False +e82f98b6090a3af3fcfc9519172f51f6b0af58eb, False +e90b8e22e998c42978d40fc56a6c82f05206f85b, False +e136a5efece6d9a91a9026540ea9e3021d77ce8b, False +e194cb80e9941fcf08be2185146d4dccf9d532c2, False +e432bd2619cb6eb619e6bf72ffa6a39892c92b7d, False +e0486f8e03441481436254ea00d35fed866c68d3, False +e837aa175298b2124bccabfbe5fd20fb0e5edca6, False +e901cfaa0ece5480b0dc999e5fbb24422b1e64dc, False +e7315a170b0ec5278e79d31226a09beef4c356ee, False +e8066bc7336a315ae887268b45ef193183993ca2, False +e8499c7327a3ad17e04a77e70979463b884846ee, False +e8547e73e4873b76eff0eb2b6488e815d7dbf41c, False +e8609a2f305e5da4b15fbac5187a228dba3e3fa3, False +e27168a25a7f6c85b3d9b1b59f8d947c12230801, False +e067340c12ea2ca15e5f1a46794bcf62d7e2f3c5, False +e073005a5e9c826bea7bb60485c7c7fc12901017, False +e81684f7e86116a81839f19a68c4b7e7b50d18ac, False +e826943dcb439733c53a13097b3e2b57c96ee382, False +e0904825b1054930b43c7c9c1a4340e24845c989, False +e1085698f8090a287a6115006d23e8f6dc68e92b, False +e35446996245a5af64cfd9979aab5befb951f032, False +ea03aed79abfb45b7809c3820237b8bb3421a690, False +ea5d508ed930bac843d729a15da80aa04a374cde, False +ea5ec37b381d5b1151ac375cd882c99af858e937, False +ea816e83b8b6895186b9b486eea931f9f70dcc29, False +ea1565e02e6aeb63328999895bdc151f4e769b16, False +eade7e268f40bd14940a6b767baf0a76d0b264e7, False +eae23b6121e974982ec521d9118e0034134c37f8, False +eaf719f5dd094990daa3e96831f847f47733587f, False +eaf49472219845b70e3354e80d499319ae5cdd0e, False +eb1f0e36c538c16711d71646749d334a48b16f40, False +eb5b7096fe0619a9831f98bacfbbe0a0130c4e8e, False +eb6a73a8cbd7df887187a2b9fad7fae1e5753d93, False +eb9f9ecf8e2aeac2f0e5a82c08ea90935e547380, False +eb18cf2561527bd086a0bcd01d4fcac8b105de07, False +eb72d804784efd835c405bd700246f9e4a35b3eb, False +eb44268ba2ef9bc41c22e47617568af49aba9ee0, False +eb73108c00be050c0676602fb83dad074cf86beb, False +ebcc3000f12eeb8d39d16b1724097ffac6d4bc13, False +ebdc1c859bc54bc67a2dba4fb9eda25ec1e3abf7, False +ebe0ebef0e0181e8a06838dec7413e9b774e2257, False +ebe8ecfad286285308f31a13d0a7e19c336f9fbe, False +ec6d929832a895b456fd6cca745a18c192c41f4b, False +ec7a01d8f3ffeb6d66d38caf2f01c3c70a80188a, False +ec11cccfce32f597425f51afcbe6aef7c2366bee, False +ec18a3aa141da6cc66cd7e3cc90909fb071481bc, False +ec44b99d2e75b597a7a8ccfcee8871dad740fa28, False +ec088effb528763544f8201107af6a5337af0141, False +ec392b4f3d4968a83003ee8c17854be753eca8c3, False +ec985fadeb6b4dbaaeb3cb9daef9dbed3eda7c35, False +ecbd9f5366385b14a468ae59696a516013286808, False +ece35962bf526099b9c50ed0105561b6bd9cba9f, False +ed3ac6c284f74cdb9212dbfb8f166c8602df82a4, False +ed95c877f692c3fad1b50a96e21ea1e02101dd42, False +ed943cd8dc7cedb573fb99c899be31601eb384f7, False +ed33312cf48e36158d4609a146edbd0249e20cd5, False +ed40273cbd12ff87551ca3cfcd6f1098d6aab3f2, False +ed0490764c26067b1a4aba532e33fda83c210146, False +edb3e40286a6913c89d7cb5309e8bef9ac24f050, False +edd29c09710bf64c66065b6d884f573bc48682f5, False +edf190bde948f0a9563431f27102d88a533a3b0a, False +edfbbc75d0b93f91f0fd530766faa55466dc74cb, False +ee4b6239be4a2513feccbe0a96c2a59cda30b1ee, False +ee06a781bc8796d23c0f27564110f677cff7779d, False +ee23a6a0d9ad4d45e17415e2536105a5172dd7be, False +ee40f3189cef7223efba7b28ffe3a49625f3fa86, False +ee98fccde070292795afc79f792b1fdccd0017e1, False +eebb35940885f412144d525d4b4552727cf0da68, False +eed320bf8d4d824d7e72c2bdb85ddd3e7b7690e3, False +eedc60093cc3519ed6b7891469ad7097bd9867c0, False +eef2341f3bed67078f416269ea1b8a6252a21d45, False +ef4ef1a757e871e774b4f1a73c55906bea1dae7f, False +ef32e2cd6dd99d883f627e238f55ad0766240d44, False +ef109fb2b790aff1ef35dbd0cfa485af31d53c9a, False +efa2a2d8993f16dea76a8ab990f447f6e08bc1b9, False +efb8d9f40f02f756ddcd767fb4bfa081cc9ac520, False +efb43e8fe3f3676e24825ad26cfd166737d1ae6c, False +efdf51d72fbd488727731660402fd776fd96fea6, False +effaac7a0ffcf86001a1f171803f46e43408d49a, False +effad41bc25621329753321234d486f782d6bc50, False +f0c1f2fc880187d25f9c334ebd100bb63400b140, False +f0d0666d5aac2ad9d896e5b6bc9c7f6be8b579ea, False +f1daaca929f58ab494442d7ffa6b60a7d886befc, False +f1de6498f43789e1c27150b3ae1f9b5bfc051775, False +f1f57d97c401d914c14d0f3d1e9e322971908b7c, False +f3b8f5e1fd45ea0496a8e5091dbd7c23e87910b2, False +f4bc230471d6dd823233c7305235aa2ed2a4f98b, False +f4ed43c5629af451d478a0b5e12b99f56a4915f7, False +f5a154e8d963064a8e43960374e4b5be523d68b0, False +f05bd29fb90510885dfdcd048dd7af8234dd7d01, False +f5bf7b0f14752c987cd026abe7b1238fb25ff9b3, False +f5d30765219e9a8ac3a49fcabd6fdd56980893e4, False +f5fd38513f70da066615a1de84e751cdf70bb91d, False +f6d1ac8f634ad47743c32018d3bda90d941a8abd, False +f06e59e6905948c5acd166a73e54bc34aaf0aada, False +f6ebdd545514a311062aabe02c49a5a4098bc3aa, False +f6f04c9393d729583a1e8d4f578e1f7a86b8982e, False +f7bca5f58a3d5259984ba28afae1a88b990b2148, False +f7dd7bd228c2036cf45951c3d956163357a6094a, False +f7e970a71d2da31cb78fc933f0db82c1c7ad60b2, False +f7ee6c8391e90873c461cc15f93f1b067270b620, False +f8aa61c06228b4a2b01786c63ce8906b2d20c1ca, False +f8b8235c6e241b3ef1922a7560736535d9c9219c, False +f8bff67d469223c8e8bf44553834bd5482a96ecc, False +f8c1be8fafd7d652e850774aa20a4e33feddea4c, False +f8c3bcfb71e73f96ad040bd10b75e3e03d2045dd, False +f8c685aa18a741e46b60a4e10150e36e61be9d0b, False +f8d72138c882ef3acb88d865d39aec766e54f82d, False +f8e52cdc85eb1f2a41718776bc0048ec0f24ae6e, False +f08f0908cdece2c62dbfcb53e7e7a7d5f3a261bf, False +f8f1526b19d05c3bc99d4d8ddf9c71b35fd2f15d, False +f9aa6e3c89a05941c1eb6859bade56571bc54a1a, False +f9b8d96daa969944c9cb86404e0092dd7cbec4c8, False +f19f4eb60204eef42a4dc3b073ef9006916a2c0b, False +f22cf30d79e9972df3854055944f0217355d62b5, False +f40c65a2d8eda8a2d4a496ccef36fc7a80e7ba04, False +f45ffeea06efea70eb1f1e8fdf53a980dbb4aa5a, False +f55b08dbf09d3ff3401465721f3e482c9b69f09c, False +f57ae04324005d0ec3f831110c9b6b0a12470d62, False +f67e0621273b7a530c00764195df0cc8ce1772af, False +f70c9cd37e4949edaa532254729fb3153ff6a76f, False +f71a047404d2f54ad39e369f3a00cb9155bc50ae, False +f71c22e2956fcc6e97ace5d6bc9334ddcd1842c1, False +f99bb0af829948601fb5bb9778b149eab30520a4, False +f189f07dd84f84ec694f21df8c35e07d392c6486, False +f227ff7f9fb5cd79326383afc70a02eb1077f19b, False +f376eee2893560e6d4e96fedfb32c29aece8ea3f, False +f379c11757a750c4d77389c528bedbe9b2a6b542, False +f401c614970ada44d00ce79eb53eb5f5522df350, False +f493df0a3c6a78965d4a87cd38a4e5c5edccdd9c, False +f497d186a78b367238f5cf26d5475f4338eded11, False +f637f110fbed653b7983d9fc6a6d53795b384461, False +f686c15418b7ef098ba67f04d746c6971ac31002, False +f839caaa9b11d4f33c01410ef8ec15ebf8423bd9, False +f1472cfab8e5e70a0112813a8379743200bf5347, False +f2724f8efec0ec93aed225a032368fd3643e8927, False +f3067e788df2d34e996d66b11be70fa7ef3ffd45, False +f5726f2e407d3c64105ae2f86398cd413bf745c6, False +f6402c587fa67fbdba921024206525ee86432449, False +f6914c27f2fc0981865d7d5bef5d7581c373a7b1, False +f7701ca4696f7d1696a11a489706cecc21ca763b, False +f84624a16fc343ce94373e0ec45362127c70451a, False +f881803c939b961e066d7282382e92e6f7f98f51, False +f927182d95b842ce7ae5898ffd9963b5c4733796, False +f992228eecd4fcee74d0ca66130701538ef6cc51, False +f5674813b8dd6c6b90f9ef8190ef2b43b46dc749, False +f018166600fafb4377f477b7fcdd41db9a87f6ce, False +f518004977c4bee3235ed41298e120a1bafb6ada, False +fa4b1d6372666ce51c59b0cb1d82d2c8fdc9d2ab, False +fa4f760152a1cf53f59ba565ae52577efbdb776a, False +fa5aecf2e29ed572333ff827b6fa0474ff3c0e8d, False +fa38edfc82a3a32fb84aadf0c8b88cd9cddf9141, False +fa786f463631d9e8ab7b794d8df610a233a445eb, False +fa4157def4e294f4fc7c68000d99b90821e47f75, False +fa5562e2e06d5c189107ed10f1c3e05552cb1bb2, False +fa4784999bded45e376e3807e60c0875ae5dd08e, False +fac95c4954bad0428057008ef9ecf18226e185c5, False +faedb8346a1f6b74360ac5a45e0be59d37e13d55, False +fb10f5c28cc55822c0b662f8b78a70f39093dec2, False +fb757ae376be045ec538f416544191b47e05bbb8, False +fb8325020112cbf94b481e2524f9975ad7a45de1, False +fba52377a4a52c8cf81d04daafa119c1b8402958, False +fbbf5204c63566f976dc25b4e87223e083c4a9a9, False +fbd0daa5fe42f6d882143f8d4010ea59c6b822f6, False +fc0ff877237969033d32f3ce1012b4221011e2c2, False +fc2ac6f89a91654a06f2116ec2f2479e3107365c, False +fc9f8dd33db0aad0f8179a4e1e448cbb46bbcddd, False +fc040dfa1829155fc4f46ad9fc0061cac3772621, False +fc3183e8a0e448595f89cfd19f59205c6d22cb3c, False +fca90fe43f5e272a58e8d20c5e205447250fb231, False +fcb104e45e5baf72955a4310cf085ad1d6a8f112, False +fccbc5d345b7887301b6644eddf52d58a37f65f2, False +fccbc696de991f8a9547b04138db49c8ae596bf0, False +fcd398b303ce33d4fc51caba1a897ad92c50e1c8, False +fdc0371ed597d39a751d5b2b28f9839580b8ec0b, False +fdd50522921663008c1e52e04905ec78aec1c622, False +fddd230b40d4c0cba89985b2a5a60f33c0cd756e, False +fdf7479cc9da8f4486359f66965869b5c58c74ed, False +fe145ac32fc7a2f829631072886fb4f6125e318d, False +fe310dfb84416974b75d6f541d2b88be2c81c8e1, False +fe7989e989af5efa49a9217c4d35c6b8752f4337, False +feb39fbc538b7a42b64d274baae3b8f10d8eb8b7, False +feb414e9a23d7083e984cd8464f13464011f15d4, False +fed2a84682713eabe2b8d0e1e950d891c7442d5f, False +fee1901e7cdc5740b0d8764fdc12a42a13787b7e, False +ff4c58065dcfa580b68f9e8de77e3d16ce2156d2, False +ff07ab7a595a3b2045d75c5d01fd93fa7fca3d16, False +ff8c73dced13943b8c4a0cfb52daa30882e42963, False +ff669aa38f01ecdebf16fc376b4d5606c1d1bef3, False +ff698a0cb15de8749c925c8aae3e8cdb8e88aa8f, False +ff950ba0a28b727b62847969d4b4e175e870416e, False +ffb44e74fd5223fe28edce2117e26f841c8c9448, False +ffdc87f5c66baa5e03861b5f8bcfb088da836bb0, False +xxxx3efb7b6bxe2ffx46ccx9dcbx8f143c18ecdc, False +xxxx5b90aecaxec62x4c50x96e4xbf3aeb5a1a6b, False +xxxx8b7566a5x40eex4434x857bx526a7ce9959c, False +xxxx30d28b68x4d7ex4815xa804x4c6279d5f156, False +xxxx54c93c4fxabe2x463exafd8xcae01106dc7d, False +xxxx78b03000x1686x4c0cxbe72xa88a248e5b9c, False +xxxx96e9160axf7eax4931x8f5cx5e8ab47299ca, False +xxxx91982f33xb278x4c6dxbc98x360785ee2a8c, False +xxxx562932e4x443cx47f7xaf2dxe60cabe20c9e, False +xxxxa823591exe017x42f7xa1fcx0ee95fc5670c, False +xxxxf2ce548bxccd4x48aex9518xaf99df64dd72, False +xxxxf3d34cd3x0738x4466xb38fxabf4cacc51f8, False +xxxxfada0856x5a66x449fxad12x9db03aed918c, False diff --git a/examples/urdfsWithNoLinks.txt b/examples/urdfsWithNoLinks.txt new file mode 100644 index 0000000000..720bb96215 --- /dev/null +++ b/examples/urdfsWithNoLinks.txt @@ -0,0 +1 @@ +xxxxfada0856x5a66x449fxad12x9db03aed918c, diff --git a/examples/viewer.py b/examples/viewer.py index 78df8231ba..809cb9b372 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -1,8 +1,11 @@ +#!/usr/bin/env python3 + # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. import ctypes +import json import math import os import string @@ -14,23 +17,87 @@ 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.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 import habitat_sim from habitat_sim import ReplayRenderer, ReplayRendererConfiguration, physics +from habitat_sim.gfx import DEFAULT_LIGHTING_KEY, DebugLineRender from habitat_sim.logging import LoggingContext, logger +from habitat_sim.utils.classes import MarkerSetsEditor, ObjectEditor, SemanticDisplay from habitat_sim.utils.common import quat_from_angle_axis +from habitat_sim.utils.namespace import hsim_physics 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) + +# from tools 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 + 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: + """ + 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 # using the magnum text module. These chars are used to # display the CPU/GPU usage data - MAX_DISPLAY_TEXT_CHARS = 256 + MAX_DISPLAY_TEXT_CHARS = 512 # 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 @@ -44,7 +111,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"] @@ -72,16 +143,6 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: 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 = { @@ -160,31 +221,122 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: # variables that track app data and CPU/GPU usage self.num_frames_to_track = 60 + # 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 + + # cache most recently loaded URDF file for quick-reload + self.cached_urdf = "" + # 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 = True - + self.simulating = False # 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() - self.debug_semantic_colors = {} + # receptacle visualization + self.receptacles = None + self.display_receptacles = False + self.show_filtered = True + 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.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? + # 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 + self.display_selected_stability_samples = True + + # collision proxy visualization + self.col_proxy_objs = None + self.col_proxies_visible = True + self.original_objs_visible = True + + # mouse raycast visualization + self.mouse_cast_results = None + self.mouse_cast_has_hits = False + + # last clicked or None for stage + self.selected_rec = None + self.ao_link_map = None + self.navmesh_dirty = False + + # index of the largest indoor island + self.largest_island_ix = -1 + + # Sim reconfigure + self.reconfigure_sim(mm) + + # load markersets for every object and ao into a cache + task_names_set = set() + task_names_set.add("faucets") + self.markersets_util = MarkerSetsEditor(self.sim, task_names_set) + + # Editing + self.obj_editor = ObjectEditor(self.sim) + + # Semantics + self.dbg_semantics = SemanticDisplay(self.sim) + + # sys.exit(0) + # load appropriate filter file for scene + self.load_scene_filter_file() + + # ----------------------------------------- + # Clutter Generation Integration: + 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_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/" + ) + self.initialize_clutter_object_set() + # ----------------------------------------- # 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" + and not self.sim_settings["viewer_ignore_navmesh"] ): self.navmesh_config_and_recompute() @@ -193,6 +345,265 @@ 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 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) + self.rec_filter_path = scene_filter_file + else: + print( + f"WARNING: No rec filter file configured for scene {self.sim.curr_scene_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 obj in self.obj_editor.sel_objs: + # find for all currently selected objects + recs = ( + self.receptacles + if (obj is None or obj.handle not in self.poh_to_rec) + else self.poh_to_rec[obj.handle] + ) + for receptacle in recs: + g_trans = receptacle.get_global_transform(self.sim) + if (g_trans.translation - pos).length() < max_dist: + # receptacles object transform should be close to the point + if isinstance(receptacle, hab_receptacle.TriangleMeshReceptacle): + r_dist = receptacle.dist_to_rec(self.sim, pos) + if r_dist < closest_rec_dist: + closest_rec_dist = r_dist + closest_rec = receptacle + else: + global_keypoints = None + if isinstance(receptacle, hab_receptacle.AABBReceptacle): + global_keypoints = ( + hsim_physics.get_global_keypoints_from_bb( + receptacle.bounds, g_trans + ) + ) + elif isinstance(receptacle, hab_receptacle.AnyObjectReceptacle): + global_keypoints = hsim_physics.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( + 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 + "cook_surface": [], + # TODO: + "height_filtered": [], + "max_height": 0, + "min_height": 0, + } + + # for rec in self.receptacles: + # 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.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)) + print(f"Exported filter annotations to {filepath}.") + + 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 + print(f"Loaded filter annotations from {filepath}") + + 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 = hsim_physics.get_obj_from_handle( + self.sim, receptacle.parent_object_handle + ).creation_attributes.handle + self.rec_to_poh[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 + ) -> 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 + object_template.is_collidable = False + 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. @@ -227,19 +638,156 @@ def draw_contact_debug(self, debug_line_render: Any): normal=camera_position - cp.position_on_b_in_ws, ) - def draw_region_debug(self, debug_line_render: Any) -> None: - """ - Draw the semantic region wireframes. - """ + def _draw_receptacle_per_obj(self, obj, debug_line_render): + # 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 = receptacle.unique_name + # 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 + + 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 obj is not None + and obj.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"], + ): + 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, + ) - 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, - ) + rec_obj = hsim_physics.get_obj_from_handle( + self.sim, receptacle.parent_object_handle + ) + key_points = [r_trans.translation] + key_points.extend( + hsim_physics.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 8 meters centered in view + if ( + c_to_r.length() < 8 + 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.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 ( + "cook_surface" in self.rec_filter_data + and rec_unique_name in self.rec_filter_data["cook_surface"] + ): + rec_color = mn.Color4(1.0, 0.66, 0.0, 1.0) # orange again + elif 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"]: + # I changed the height filter from orange to dark purple + rec_color = mn.Color4(0.5, 0, 0.5, 1.0) + # 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" + # ] + # ) + + receptacle.debug_draw(self.sim, color=rec_color) + if True: + t_form = receptacle.get_global_transform(self.sim) + debug_line_render.push_transform(t_form) + debug_line_render.draw_transformed_line( + mn.Vector3(0), receptacle.up, mn.Color4.cyan() + ) + debug_line_render.pop_transform() + + def draw_receptacles(self, debug_line_render): + for obj in self.obj_editor.sel_objs: + self._draw_receptacle_per_obj(obj, debug_line_render=debug_line_render) def debug_draw(self): """ @@ -250,17 +798,31 @@ def debug_draw(self): proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) self.sim.physics_debug_draw(proj_mat) - debug_line_render = self.sim.get_debug_line_render() + debug_line_render: DebugLineRender = 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: - self.debug_semantic_colors[region.id] = mn.Color4( - mn.Vector3(np.random.random(3)) - ) - self.draw_region_debug(debug_line_render) + # draw semantic information + self.dbg_semantics.draw_region_debug(debug_line_render=debug_line_render) + + # draw markersets information + if self.markersets_util.marker_sets_per_obj is not None: + self.markersets_util.draw_marker_sets_debug( + debug_line_render, + self.render_camera.render_camera.node.absolute_translation, + ) + if self.receptacles is not None and self.display_receptacles: + self.draw_receptacles(debug_line_render) + + self.obj_editor.draw_selected_objects(debug_line_render) + # mouse raycast circle + if self.mouse_cast_has_hits: + debug_line_render.draw_circle( + translation=self.mouse_cast_results.hits[0].point, + radius=0.005, + color=mn.Color4(mn.Vector3(1.0), 1.0), + normal=self.mouse_cast_results.hits[0].normal, + ) def draw_event( self, @@ -272,6 +834,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( @@ -290,9 +856,21 @@ 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() - + if self.navmesh_dirty: + self.navmesh_config_and_recompute() # 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 @@ -362,7 +940,26 @@ def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: ) return agent_config - def reconfigure_sim(self) -> None: + 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: """ 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 @@ -370,6 +967,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() @@ -381,7 +979,7 @@ def reconfigure_sim(self) -> None: 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 + self.cfg.sim_cfg.scene_light_setup = DEFAULT_LIGHTING_KEY if self.sim is None: self.tiled_sims = [] @@ -398,6 +996,10 @@ def reconfigure_sim(self) -> None: 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( @@ -428,6 +1030,9 @@ def reconfigure_sim(self) -> None: for composite_file in sim_settings["composite_files"]: self.replay_renderer.preload_file(composite_file) + self.ao_link_map = hsim_physics.get_ao_link_id_map(self.sim) + self.dbv = DebugVisualizer(self.sim) + Timer.start() self.step = -1 @@ -457,10 +1062,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] @@ -480,6 +1084,223 @@ 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 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 = hsim_physics.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 add_objects_to_receptacles(self, alt_pressed: bool, shift_pressed: bool): + rom = self.sim.get_rigid_object_manager() + # add objects to the selected receptacle or remove all 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 len(self.obj_editor.sel_objs) != 0: + rec_set = [] + for obj in self.obj_editor.sel_objs: + tmp_list = [ + rec + for rec in self.receptacles + if obj.handle == rec.parent_object_handle + ] + rec_set.extend(tmp_list) + 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), + ) + obj_samp.receptacle_instances = self.receptacles + 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) + 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.") + def key_press_event(self, event: Application.KeyEvent) -> None: """ Handles `Application.KeyEvent` on a key press by performing the corresponding functions. @@ -498,46 +1319,21 @@ def key_press_event(self, event: Application.KeyEvent) -> None: event.accepted = True self.exit_event(Application.ExitEvent) return - - elif key == pressed.H: - self.print_help_text() - elif key == pressed.J: - logger.info( - f"Toggle Region Draw from {self.semantic_region_debug_draw } to {not self.semantic_region_debug_draw}" - ) - # Toggle visualize semantic bboxes. Currently only regions supported - self.semantic_region_debug_draw = not self.semantic_region_debug_draw + elif key == pressed.ONE: + # save scene instance + self.obj_editor.save_current_scene() + print("Saved modified scene instance JSON to original location.") + elif key == pressed.TWO: + # Undo any edits + self.obj_editor.undo_edit() + + 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: @@ -557,73 +1353,66 @@ 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.B: + # Change editor values + self.obj_editor.change_edit_vals(toggle=shift_pressed) + 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: + self.contact_debug_draw = not self.contact_debug_draw + log_str = f"Command: toggle contact debug draw: {self.contact_debug_draw}" + if self.contact_debug_draw: # perform a discrete collision detection pass and enable contact debug drawing to visualize the results - 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: - # 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, - 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) + log_str = f"{log_str}: performing discrete collision detection and visualize active contacts." + self.sim.perform_discrete_collision_detection() + logger.info(log_str) + + elif key == pressed.E: + # Cyle through semantics display + info_str = self.dbg_semantics.cycle_semantic_region_draw() + logger.info(info_str) + + elif key == pressed.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: - logger.warn("Load URDF: input file not found. Aborting.") + self.show_filtered = not self.show_filtered + print(f"self.show_filtered = {self.show_filtered}") + + elif key == pressed.G: + # Change editor mode + self.obj_editor.change_edit_mode(toggle=shift_pressed) + + elif key == pressed.H: + self.print_help_text() + + elif key == pressed.I: + self.navmesh_dirty = self.obj_editor.edit_up( + self.navmesh_dirty, toggle=shift_pressed + ) + + elif key == pressed.J: + self.navmesh_dirty = self.obj_editor.edit_left(self.navmesh_dirty) + + elif key == pressed.K: + self.navmesh_dirty = self.obj_editor.edit_down( + self.navmesh_dirty, toggle=shift_pressed + ) + + elif key == pressed.L: + self.navmesh_dirty = self.obj_editor.edit_right(self.navmesh_dirty) elif key == pressed.M: - self.cycle_mouse_mode() - logger.info(f"Command: mouse mode set to {self.mouse_interaction}") + if shift_pressed: + # Save all markersets that have been changed + self.markersets_util.save_all_dirty_markersets() + else: + 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 @@ -632,8 +1421,12 @@ 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() + + print(f"Largest indoor island index = {self.largest_island_ix}") new_agent_state.position = ( - self.sim.pathfinder.get_random_navigable_point() + self.sim.pathfinder.get_random_navigable_point( + island_index=self.largest_island_ix + ) ) new_agent_state.rotation = quat_from_angle_axis( self.sim.random.uniform_float(0, 2.0 * np.pi), @@ -654,6 +1447,175 @@ def key_press_event(self, event: Application.KeyEvent) -> None: 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.P: + # If shift pressed then open, otherwise close + # If alt pressed then selected, otherwise all + self.obj_editor.set_ao_joint_states( + do_open=shift_pressed, selected=alt_pressed + ) + if not shift_pressed: + # if closing then redo navmesh + self.navmesh_config_and_recompute() + + elif key == pressed.Q: + self.add_objects_to_receptacles( + alt_pressed=alt_pressed, shift_pressed=shift_pressed + ) + + elif key == pressed.R: + # Reload current scene + self.cycleScene(False, shift_pressed=shift_pressed) + + elif key == pressed.T: + if shift_pressed: + # open all the AO default links + aos = hsim_physics.get_all_ao_objects(self.sim) + for ao in aos: + default_link = hsim_physics.get_ao_default_link(ao, True) + hsim_physics.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 + # 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, + # 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.U: + # Remove object + # 'Remove' all selected objects by moving them out of view. + # Removal only becomes permanent when scene is saved + self.obj_editor.remove_sel_objects() + + self.navmesh_config_and_recompute() + + 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.Y and self.selected_rec is not None: + if self.selected_rec.unique_name in self.rec_filter_data["cook_surface"]: + print(self.selected_rec.unique_name + " removed from 'cook_surface'") + self.rec_filter_data["cook_surface"].remove( + self.selected_rec.unique_name + ) + self.selected_rec = None + else: + print(self.selected_rec.unique_name + " added to 'cook_surface'") + self.rec_filter_data["cook_surface"].append( + self.selected_rec.unique_name + ) + self.selected_rec = None + # update map of moving/looking keys which are currently pressed if key in self.pressed: self.pressed[key] = True @@ -674,12 +1636,184 @@ def key_release_event(self, event: Application.KeyEvent) -> None: event.accepted = True self.redraw() + def calc_mouse_cast_results(self, screen_location: mn.Vector3) -> None: + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(screen_location)) + mouse_cast_results = self.sim.cast_ray(ray=ray) + self.mouse_cast_has_hits = ( + mouse_cast_results is not None and mouse_cast_results.has_hits() + ) + self.mouse_cast_results = mouse_cast_results + + def mouse_look_handler( + self, is_right_btn: bool, shift_pressed: bool, alt_pressed: bool + ): + if is_right_btn: + sel_obj = None + self.selected_rec = None + hit_info = self.mouse_cast_results.hits[0] + hit_id = hit_info.object_id + # right click in look mode to print object information + if hit_id == habitat_sim.stage_id: + print("This is the stage.") + else: + obj = hsim_physics.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] + sel_obj = obj + print(f"Object: {obj.handle}") + if obj.is_articulated: + print("links = ") + for obj_id, link_id in obj.link_object_ids.items(): + print(f" {link_id} : {obj_id} : {obj.get_link_name(link_id)}") + if hit_id == obj_id: + print(" !^!") + if self.receptacles is not None: + for rec in self.receptacles: + if rec.parent_object_handle == obj.handle: + print(f" - Receptacle: {rec.name}") + if shift_pressed: + 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_poh[new_rec] = obj.creation_attributes.handle + self.selected_rec = self.get_closest_tri_receptacle(hit_info.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(hit_info.point) + if filtered_rec is not None: + filtered_rec_name = filtered_rec.unique_name + 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 + ) + elif obj.is_articulated: + # get the default link + default_link = hsim_physics.get_ao_default_link(obj, True) + if default_link is None: + print("Selected AO has no default link.") + else: + if hsim_physics.link_is_open(obj, default_link, 0.05): + hsim_physics.close_link(obj, default_link) + else: + hsim_physics.open_link(obj, default_link) + + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(sel_obj) + + def mouse_grab_handler(self, is_right_btn: bool): + ao_link = -1 + hit_info = self.mouse_cast_results.hits[0] + + if hit_info.object_id > habitat_sim.stage_id: + obj = hsim_physics.get_obj_from_id( + self.sim, hit_info.object_id, self.ao_link_map + ) + + if obj is None: + raise AssertionError( + "hit object_id is not valid. Did not find object or link." + ) + + if obj.object_id == hit_info.object_id: + # ro or ao base + object_pivot = obj.transformation.inverted().transform_point( + hit_info.point + ) + 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) + ) + 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 is_right_btn: + constraint_settings.constraint_type = physics.RigidConstraintType.Fixed + + grip_depth = ( + hit_info.point + - self.render_camera.render_camera.node.absolute_translation + ).length() + + self.mouse_grabber = MouseGrabber( + constraint_settings, + grip_depth, + self.sim, + ) + 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. """ + self.calc_mouse_cast_results(event.position) + button = Application.MouseMoveEvent.Buttons # if interactive mode -> LOOK MODE if event.buttons == button.LEFT and self.mouse_interaction == MouseMode.LOOK: @@ -712,91 +1846,31 @@ 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) + self.calc_mouse_cast_results(event.position) # 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 > 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) - - 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() + if physics_enabled and self.mouse_cast_has_hits: + if self.mouse_interaction == MouseMode.GRAB: + self.mouse_grab_handler(event.button == button.RIGHT) + elif self.mouse_interaction == MouseMode.LOOK: + self.mouse_look_handler( + event.button == button.RIGHT, + shift_pressed=shift_pressed, + alt_pressed=alt_pressed, + ) - 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.MARKER: + # hit_info = self.mouse_cast_results.hits[0] + sel_obj = self.markersets_util.place_marker_at_hit_location( + self.mouse_cast_results.hits[0], + self.ao_link_map, + event.button == button.LEFT, + ) + # clear all selected objects and set to found obj + self.obj_editor.set_sel_obj(sel_obj) self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() @@ -824,11 +1898,19 @@ def mouse_scroll_event(self, event: Application.MouseScrollEvent) -> None: # if interactive mode is False -> LOOK MODE if self.mouse_interaction == MouseMode.LOOK: # use shift for fine-grained zooming + # TODO : need to support camera handling like done for spot here. See spot_viewer.py + # if alt_pressed: + # # move camera in/out + # mod_val = 0.3 if shift_pressed else 0.15 + # scroll_delta = scroll_mod_val * mod_val + # self.camera_distance -= scroll_delta + # else: 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() + # self.redraw() elif self.mouse_interaction == MouseMode.GRAB and self.mouse_grabber: # adjust the depth @@ -852,6 +1934,15 @@ def mouse_scroll_event(self, event: Application.MouseScrollEvent) -> None: # update location of grabbed object self.mouse_grabber.grip_depth += scroll_delta self.update_grab_position(self.get_mouse_position(event.position)) + elif self.mouse_interaction == MouseMode.MARKER: + self.markersets_util.cycle_current_taskname(scroll_mod_val > 0) + # marker_mod = 1 if scroll_mod_val > 0 else -1 + # self.current_markerset_taskset_idx = ( + # self.current_markerset_taskset_idx + # + len(self.markerset_taskset_names) + # + marker_mod + # ) % len(self.markerset_taskset_names) + self.redraw() event.accepted = True @@ -899,6 +1990,8 @@ def cycle_mouse_mode(self) -> None: if self.mouse_interaction == MouseMode.LOOK: self.mouse_interaction = MouseMode.GRAB elif self.mouse_interaction == MouseMode.GRAB: + self.mouse_interaction = MouseMode.MARKER + elif self.mouse_interaction == MouseMode.MARKER: self.mouse_interaction = MouseMode.LOOK def navmesh_config_and_recompute(self) -> None: @@ -911,11 +2004,36 @@ def navmesh_config_and_recompute(self) -> None: 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 + + # 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.append((ao, 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 + for ao, ao_orig_motion_type in ao_motion_types: + ao.motion_type = ao_orig_motion_type + + self.largest_island_ix = get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) + + self.navmesh_dirty = False + def exit_event(self, event: Application.ExitEvent): """ Overrides exit_event to properly close the Simulator before exiting the @@ -944,12 +2062,20 @@ def draw_text(self, sensor_spec): mouse_mode_string = "LOOK" elif self.mouse_interaction == MouseMode.GRAB: mouse_mode_string = "GRAB" + elif self.mouse_interaction == MouseMode.MARKER: + mouse_mode_string = "MARKER" + + edit_string = self.obj_editor.edit_disp_str() 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} Mouse Interaction Mode: {mouse_mode_string} +{edit_string} +Selected MarkerSets TaskSet name : {self.markersets_util.get_current_taskname()} +Unstable Objects: {self.num_unstable_objects} of {len(self.clutter_object_instances)} """ ) self.shader.draw(self.window_text.mesh) @@ -973,6 +2099,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: @@ -985,7 +2115,23 @@ def print_help_text(self) -> None: (+CTRL) rotate object fixed constraint frame (pitch) (+ALT+CTRL) rotate object fixed constraint frame (roll) (+SHIFT) amplify scroll magnitude - +In MARKER mode : + + +Edit Commands : +--------------- + '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 Edit Mode: Move Object mode is selected : + - 'j'/'l' : move the object along global X axis. + - 'i'/'k' : move the object along global Z axis. + (+SHIFT): move the object up/down (global Y axis) + When Edit Mode: Rotate Object mode is selected : + - 'j'/'l' : rotate the object around global Y axis. + - 'i'/'k' : arrow keys: rotate the object around global Z axis. + (+SHIFT): rotate the object around global X axis. + - 'u': delete the selected object Key Commands: ------------- @@ -999,22 +2145,39 @@ def print_help_text(self) -> None: arrow keys: Turn the agent's body left/right and camera look up/down. Utilities: + '1 (one)': Save current scene instance, overwriting existing scene instance. '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. - 'j' Toggle Semantic visualization bounds (currently only Semantic Region annotations) + 'c': Toggle the contact point debug render overlay on/off. If toggled to true, + then run a discrete collision detection pass and render a debug wireframe overlay + showing active contact points and normals (yellow=fixed length normals, red=collision distances). + 'p' Modify AO link states : + (+SHIFT) : Open Selected/All AOs + (-SHIFT) : Close Selected/All AOs + (+ALT) : Modify Selected AOs + (-ALT) : Modify All AOs + 'e' Toggle Semantic visualization bounds (currently only Semantic Region annotations) 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. + 'q': Sample an object placement from the currently selected object or receptacle. + (+SHIFT) Remove all previously sampled objects. + (+ALT) Sample from all "active" unfiltered Receptacles. + ===================================================== """ ) @@ -1024,6 +2187,7 @@ class MouseMode(Enum): LOOK = 0 GRAB = 1 MOTION = 2 + MARKER = 3 class MouseGrabber: @@ -1133,6 +2297,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 the 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 + +# threads = [] +# for obj_handle in objects_in_scene: +# run_cpo_for_obj(obj_handle) +# # threads.append(threading.Thread(target=run_cpo_for_obj, args=(obj_handle,))) +# for thread in threads: +# thread.start() + + if __name__ == "__main__": import argparse @@ -1152,6 +2355,17 @@ def next_frame() -> None: 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", + help="Initialize and run the CPO for the current scene.", + ) parser.add_argument( "--disable-physics", action="store_true", @@ -1184,15 +2398,21 @@ def next_frame() -> None: 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=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.", ) @@ -1217,8 +2437,20 @@ def next_frame() -> None: sim_settings["composite_files"] = args.composite_files sim_settings["window_width"] = args.width sim_settings["window_height"] = args.height - sim_settings["default_agent_navmesh"] = False + 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 + + 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/src/esp/assets/ResourceManager.cpp b/src/esp/assets/ResourceManager.cpp index 3227d510b6..f0ad6df3fd 100644 --- a/src/esp/assets/ResourceManager.cpp +++ b/src/esp/assets/ResourceManager.cpp @@ -2885,7 +2885,7 @@ bool ResourceManager::instantiateAssetsOnDemand( // object has acquired a copy of its parent attributes. No object should // ever have a copy of attributes with isDirty == true - any editing of // attributes for objects requires object rebuilding. - if (objectAttributes->getIsDirty()) { + if (objectAttributes->getFilePathsAreDirty()) { CORRADE_ASSERT( (getObjectAttributesManager()->registerObject( objectAttributes, objectTemplateHandle) != ID_UNDEFINED), diff --git a/src/esp/bindings/AttributesBindings.cpp b/src/esp/bindings/AttributesBindings.cpp index e086ea896c..47e9e876f6 100644 --- a/src/esp/bindings/AttributesBindings.cpp +++ b/src/esp/bindings/AttributesBindings.cpp @@ -453,7 +453,11 @@ void initAttributesBindings(py::module& m) { R"(Class name of Attributes template.)") .def_property_readonly( "csv_info", &AbstractAttributes::getObjectInfo, - R"(Comma-separated informational string describing this Attributes template)"); + R"(Comma-separated informational string describing this Attributes template)") + .def_property_readonly( + "filenames_are_dirty", &AbstractAttributes::getFilePathsAreDirty, + R"(Whether filenames or paths in this attributes have been changed requiring + re-registration before they can be used to create an object. )"); // Attributes should only use named properties or subconfigurations to set // specific values, to guarantee essential value type integrity. This will @@ -680,11 +684,7 @@ void initAttributesBindings(py::module& m) { .def_property( "is_collidable", &ObjectAttributes::getIsCollidable, &ObjectAttributes::setIsCollidable, - R"(Whether constructions built from this template are collidable upon initialization.)") - .def_property_readonly( - "is_dirty", &AbstractObjectAttributes::getIsDirty, - R"(Whether values in this attributes have been changed requiring - re-registration before they can be used to create an object. )"); + R"(Whether constructions built from this template are collidable upon initialization.)"); // ==== ObjectAttributes ==== py::class_( diff --git a/src/esp/bindings/ConfigBindings.cpp b/src/esp/bindings/ConfigBindings.cpp index 78425448e8..c2a4db78f0 100644 --- a/src/esp/bindings/ConfigBindings.cpp +++ b/src/esp/bindings/ConfigBindings.cpp @@ -152,7 +152,7 @@ void initConfigBindings(py::module& m) { R"(Returns whether or not this Configuration has the passed key. Does not check subconfigurations.)", "key"_a) .def( - "has_key_to_type", &Configuration::hasKeyOfType, + "has_key_to_type", &Configuration::hasKeyToValOfType, R"(Returns whether passed key points to a value of specified ConfigValType)", "key"_a, "value_type"_a) .def( diff --git a/src/esp/bindings/SimBindings.cpp b/src/esp/bindings/SimBindings.cpp index 41e990e57b..4e2600047c 100644 --- a/src/esp/bindings/SimBindings.cpp +++ b/src/esp/bindings/SimBindings.cpp @@ -164,7 +164,7 @@ void initSimBindings(py::module& m) { R"(Use gfx_replay_manager for replay recording and playback.)") .def("seed", &Simulator::seed, "new_seed"_a) .def("reconfigure", &Simulator::reconfigure, "configuration"_a) - .def("reset", &Simulator::reset) + .def("reset", [](Simulator& self) { self.reset(false); }) .def( "close", &Simulator::close, "destroy"_a = true, R"(Free all loaded assets and GPU contexts. Use destroy=true except where noted in tutorials/async_rendering.py.)") diff --git a/src/esp/core/Configuration.cpp b/src/esp/core/Configuration.cpp index b95b1f735d..d5992aafba 100644 --- a/src/esp/core/Configuration.cpp +++ b/src/esp/core/Configuration.cpp @@ -207,18 +207,35 @@ bool operator==(const ConfigValue& a, const ConfigValue& b) { if (a._typeAndFlags != b._typeAndFlags) { return false; } + const auto dataType = a.getType(); // Pointer-backed data types need to have _data dereffed - if (isConfigValTypePointerBased(a.getType())) { - return pointerBasedConfigTypeHandlerFor(a.getType()) - .comparator(a._data, b._data); + if (isConfigValTypePointerBased(dataType)) { + return pointerBasedConfigTypeHandlerFor(dataType).comparator(a._data, + b._data); } - // Trivial type : a._data holds the actual value - // _data array will always hold only legal data, since a ConfigValue should - // never change type. + // By here we know the type is a trivial type and that the types for both + // values are equal + if (a.reqsFuzzyCompare()) { + // Type is specified to require fuzzy comparison + switch (dataType) { + case ConfigValType::Double: { + return Mn::Math::equal(a.get(), b.get()); + } + default: { + CORRADE_ASSERT_UNREACHABLE( + "Unknown/unsupported Type in ConfigValue::operator==()", ""); + } + } + } + + // Trivial non-fuzzy-comparison-requiring type : a._data holds the actual + // value _data array will always hold only legal data, since a ConfigValue + // should never change type. return std::equal(std::begin(a._data), std::end(a._data), std::begin(b._data)); -} + +} // ConfigValue::operator== bool operator!=(const ConfigValue& a, const ConfigValue& b) { return !(a == b); @@ -236,7 +253,7 @@ std::string ConfigValue::getAsString() const { return std::to_string(get()); } case ConfigValType::Double: { - return std::to_string(get()); + return Cr::Utility::formatString("{}", get()); } case ConfigValType::String: { return get(); @@ -295,7 +312,8 @@ std::string ConfigValue::getAsString() const { io::JsonGenericValue ConfigValue::writeToJsonObject( io::JsonAllocator& allocator) const { - // unknown is checked before this function is called, so does not need support + // unknown is checked before this function is called, so does not need + // support switch (getType()) { case ConfigValType::Boolean: { return io::toJsonValue(get(), allocator); @@ -487,8 +505,8 @@ int Configuration::loadOneConfigFromJson(int numConfigSettings, } else { // The array does not match any currently supported magnum // objects, so place in indexed subconfig of values. - // decrement count by 1 - the recursive subgroup load will count all the - // values. + // decrement count by 1 - the recursive subgroup load will count all + // the values. --numConfigSettings; // create a new subgroup std::shared_ptr subGroupPtr = @@ -496,8 +514,8 @@ int Configuration::loadOneConfigFromJson(int numConfigSettings, // load array into subconfig numConfigSettings += subGroupPtr->loadFromJsonArray(jsonObj); } - // value in array is a number of specified length, else it is a string, an - // object or a nested array + // value in array is a number of specified length, else it is a string, + // an object or a nested array } else { // decrement count by 1 - the recursive subgroup load will count all the // values. @@ -584,8 +602,8 @@ void Configuration::writeValuesToJson(io::JsonGenericValue& jsonObj, << "`, so nothing will be written to JSON for this key."; } else if (valIter->second.shouldWriteToFile()) { - // Create Generic value for key, using allocator, to make sure its a copy - // and lives long enough + // Create Generic value for key, using allocator, to make sure its a + // copy and lives long enough writeValueToJsonInternal(valIter->second, valIter->first.c_str(), jsonObj, allocator); } else { @@ -602,8 +620,8 @@ void Configuration::writeSubconfigsToJson(io::JsonGenericValue& jsonObj, ++cfgIter) { // only save if subconfig tree has value entries if (cfgIter->second->getConfigTreeNumValues() > 0) { - // Create Generic value for key, using allocator, to make sure its a copy - // and lives long enough + // Create Generic value for key, using allocator, to make sure its a + // copy and lives long enough io::JsonGenericValue name{cfgIter->first.c_str(), allocator}; io::JsonGenericValue subObj = cfgIter->second->writeToJsonObject(allocator); @@ -663,9 +681,9 @@ void Configuration::setSubconfigValsOfTypeInVector( /** * @brief Retrieves a shared pointer to a copy of the subConfig @ref * esp::core::config::Configuration that has the passed @p name . This will - * create a pointer to a new sub-configuration if none exists already with that - * name, but will not add this configuration to this Configuration's internal - * storage. + * create a pointer to a new sub-configuration if none exists already with + * that name, but will not add this configuration to this Configuration's + * internal storage. * * @param name The name of the configuration to retrieve. * @return A pointer to a copy of the configuration having the requested @@ -783,6 +801,52 @@ std::vector Configuration::findValue( return breadcrumbs; } +void Configuration::overwriteWithConfig( + const std::shared_ptr& src) { + if (src->getNumEntries() == 0) { + return; + } + // copy every element over from src + for (const auto& elem : src->valueMap_) { + valueMap_[elem.first] = elem.second; + } + // merge subconfigs + for (const auto& subConfig : src->configMap_) { + const auto name = subConfig.first; + // make if DNE and merge src subconfig + addOrEditSubgroup(name).first->second->overwriteWithConfig( + subConfig.second); + } +} // Configuration::overwriteWithConfig + +void Configuration::filterFromConfig( + const std::shared_ptr& src) { + if (src->getNumEntries() == 0) { + return; + } + // filter out every element that is present with the same value in both src + // and this. + for (const auto& elem : src->valueMap_) { + ValueMapType::const_iterator mapIter = valueMap_.find(elem.first); + // if present and has the same data, erase this configuration's data + if ((mapIter != valueMap_.end()) && (mapIter->second == elem.second)) { + valueMap_.erase(mapIter); + } + } + // repeat process on all subconfigs of src that are present in this. + for (const auto& subConfig : src->configMap_) { + // find if this has subconfig of same name + ConfigMapType::iterator mapIter = configMap_.find(subConfig.first); + if (mapIter != configMap_.end()) { + mapIter->second->filterFromConfig(subConfig.second); + // remove the subconfig if it has no entries after filtering + if (mapIter->second->getNumEntries() == 0) { + configMap_.erase(mapIter); + } + } + } +} // Configuration::filterFromConfig + Configuration& Configuration::operator=(const Configuration& otr) { if (this != &otr) { configMap_.clear(); @@ -820,7 +884,7 @@ Mn::Debug& operator<<(Mn::Debug& debug, const Configuration& cfg) { bool operator==(const Configuration& a, const Configuration& b) { if ((a.getNumSubconfigs() != b.getNumSubconfigs()) || - (a.getNumValues() != b.getNumValues())) { + (a.getNumVisibleValues() != b.getNumVisibleValues())) { return false; } for (const auto& entry : a.configMap_) { diff --git a/src/esp/core/Configuration.h b/src/esp/core/Configuration.h index e5234109a2..6844144b62 100644 --- a/src/esp/core/Configuration.h +++ b/src/esp/core/Configuration.h @@ -133,6 +133,13 @@ enum ConfigValStatus : uint64_t { * properly read from or written to file otherwise. */ isTranslated = 1ULL << 34, + + /** + * @brief This @ref ConfigValue requires manual fuzzy comparison (i.e. floating + * point scalar type) using the Magnum::Math::equal method. Magnum types + * already perform fuzzy comparison. + */ + reqsFuzzyComparison = 1ULL << 35, }; // enum class ConfigValStatus /** @@ -157,6 +164,24 @@ constexpr bool isConfigValTypeNonTrivial(ConfigValType type) { static_cast(ConfigValType::_nonTrivialTypes); } +/** + * @brief Function template to return whether the value requires fuzzy + * comparison or not. + */ +template +constexpr bool useFuzzyComparisonFor() { + // Default for all types is no. + return false; +} + +/** + * @brief Specify that @ref ConfigValType::Double scalar floating point values require fuzzy comparison + */ +template <> +constexpr bool useFuzzyComparisonFor() { + return true; +} + /** * @brief Function template to return type enum for specified type. All * supported types should have a specialization of this function handling their @@ -272,8 +297,7 @@ constexpr ConfigValType configValTypeFor() { /** * @brief Stream operator to support display of @ref ConfigValType enum tags */ -MAGNUM_EXPORT Mn::Debug& operator<<(Mn::Debug& debug, - const ConfigValType& value); +Mn::Debug& operator<<(Mn::Debug& debug, const ConfigValType& value); /** * @brief This class uses an anonymous tagged union to store values of different @@ -355,7 +379,8 @@ class ConfigValue { } /** - * @brief Get this ConfigVal's value. Type is stored as a Pointer. + * @brief Get this ConfigVal's value. For Types that are stored in _data as a + * Pointer. */ template EnableIf()), const T&> @@ -367,7 +392,8 @@ class ConfigValue { } /** - * @brief Get this ConfigVal's value. Type is stored directly in buffer. + * @brief Get this ConfigVal's value. For Types that are stored directly in + * buffer. */ template EnableIf()), const T&> @@ -413,6 +439,9 @@ class ConfigValue { //_data should be destructed at this point, construct a new value setInternalTyped(value); + // set whether this type requires fuzzy comparison or not + setReqsFuzzyCompare(useFuzzyComparisonFor()); + } // ConfigValue::setInternal /** @@ -621,6 +650,29 @@ class ConfigValue { setState(ConfigValStatus::isTranslated, isTranslated); } + /** + * @brief Check whether this ConfigVal requires a fuzzy comparison for + * equality (i.e. for a scalar double). + * + * The comparisons for such a type + * should use Magnum::Math::equal to be consistent with similarly configured + * magnum types. + */ + inline bool reqsFuzzyCompare() const { + return getState(ConfigValStatus::reqsFuzzyComparison); + } + /** + * @brief Check whether this ConfigVal requires a fuzzy comparison for + * equality (i.e. for a scalar double). + * + * The comparisons for such a type + * should use Magnum::Math::equal to be consistent with similarly configured + * magnum types. + */ + inline void setReqsFuzzyCompare(bool fuzzyCompare) { + setState(ConfigValStatus::reqsFuzzyComparison, fuzzyCompare); + } + /** * @brief Whether or not this @ref ConfigValue should be written to file during * common execution. The reason we may not want to do this might be that the @@ -657,7 +709,7 @@ class ConfigValue { /** * @brief provide debug stream support for @ref ConfigValue */ -MAGNUM_EXPORT Mn::Debug& operator<<(Mn::Debug& debug, const ConfigValue& value); +Mn::Debug& operator<<(Mn::Debug& debug, const ConfigValue& value); /** * @brief This class holds Configuration data in a map of ConfigValues, and @@ -697,7 +749,7 @@ class Configuration { : configMap_(std::move(otr.configMap_)), valueMap_(std::move(otr.valueMap_)) {} // move ctor - // virtual destructor set to that pybind11 recognizes attributes inheritance + // virtual destructor set so that pybind11 recognizes attributes inheritance // from Configuration to be polymorphic virtual ~Configuration() = default; @@ -751,6 +803,8 @@ class Configuration { return {}; } + // ****************** Value Status ****************** + /** * @brief Return the @ref ConfigValType enum representing the type of the * value referenced by the passed @p key or @ref ConfigValType::Unknown @@ -765,6 +819,46 @@ class Configuration { return ConfigValType::Unknown; } + /** + * @brief Returns whether or not the @ref ConfigValue specified + * by @p key is a default/initialization value or was intentionally set. + */ + bool isDefaultVal(const std::string& key) const { + ValueMapType::const_iterator mapIter = valueMap_.find(key); + if (mapIter != valueMap_.end()) { + return mapIter->second.isDefaultVal(); + } + ESP_ERROR() << "Key :" << key << "not present in Configuration."; + return false; + } + + /** + * @brief Returns whether or not the @ref ConfigValue specified + * by @p key is a hidden value intended to be be only used internally. + */ + bool isHiddenVal(const std::string& key) const { + ValueMapType::const_iterator mapIter = valueMap_.find(key); + if (mapIter != valueMap_.end()) { + return mapIter->second.isHiddenVal(); + } + ESP_ERROR() << "Key :" << key << "not present in Configuration."; + return false; + } + + /** + * @brief Returns whether or not the @ref ConfigValue specified + * by @p key is a translated value, meaning a string that corresponds to, and + * is translated into, an enum value for consumption. + */ + bool isTranslated(const std::string& key) const { + ValueMapType::const_iterator mapIter = valueMap_.find(key); + if (mapIter != valueMap_.end()) { + return mapIter->second.isTranslated(); + } + ESP_ERROR() << "Key :" << key << "not present in Configuration."; + return false; + } + // ****************** String Conversion ****************** /** @@ -1133,10 +1227,26 @@ class Configuration { } /** - * @brief returns number of values in this Configuration. + * @brief Returns number of values in this Configuration. */ int getNumValues() const { return valueMap_.size(); } + /** + * @brief Returns number of non-hidden values in this Configuration. This is + * necessary for determining whether or not configurations are "effectively" + * equal, where they contain the same data but may vary in number + * internal-use-only fields. + */ + int getNumVisibleValues() const { + int numVals = 0; + for (const auto& val : valueMap_) { + if (!val.second.isHiddenVal()) { + numVals += 1; + } + } + return numVals; + } + /** * @brief Return total number of values held by this Configuration and all * its subconfigs. @@ -1163,7 +1273,8 @@ class Configuration { * @param desiredType the @ref ConfigValType to compare the value's type to * @return Whether @p key references a value that is of @p desiredType. */ - bool hasKeyOfType(const std::string& key, ConfigValType desiredType) { + bool hasKeyToValOfType(const std::string& key, + ConfigValType desiredType) const { ValueMapType::const_iterator mapIter = valueMap_.find(key); return (mapIter != valueMap_.end() && (mapIter->second.getType() == desiredType)); @@ -1247,7 +1358,6 @@ class Configuration { * @return A pointer to a copy of the Configuration having the requested * name, cast to the appropriate type, or nullptr if not found. */ - template std::shared_ptr getSubconfigCopy(const std::string& cfgName) const { static_assert(std::is_base_of::value, @@ -1371,27 +1481,24 @@ class Configuration { /** * @brief Merges Configuration pointed to by @p src into this - * Configuration, including all subconfigs. Passed config overwrites + * Configuration, including all subconfigs. Passed config overwrites * existing data in this config. * @param src The source of Configuration data we wish to merge into this * Configuration. */ - void overwriteWithConfig(const std::shared_ptr& src) { - if (src->getNumEntries() == 0) { - return; - } - // copy every element over from src - for (const auto& elem : src->valueMap_) { - valueMap_[elem.first] = elem.second; - } - // merge subconfigs - for (const auto& subConfig : src->configMap_) { - const auto name = subConfig.first; - // make if DNE and merge src subconfig - addOrEditSubgroup(name).first->second->overwriteWithConfig( - subConfig.second); - } - } + void overwriteWithConfig(const std::shared_ptr& src); + + /** + * @brief Performs the opposite operation to @ref Configuration::overwriteWithConfig. + * All values and subconfigs in the passed Configuration will be removed from + * this config unless the data they hold is different. Any empty subconfigs + * will be removed as well. + * + * @param src The source of Configuration data we wish to prune from this + * Configuration. + */ + + void filterFromConfig(const std::shared_ptr& src); /** * @brief Returns a const iterator across the map of values. @@ -1695,8 +1802,7 @@ class Configuration { /** * @brief provide debug stream support for a @ref Configuration */ -MAGNUM_EXPORT Mn::Debug& operator<<(Mn::Debug& debug, - const Configuration& value); +Mn::Debug& operator<<(Mn::Debug& debug, const Configuration& value); template <> std::vector Configuration::getSubconfigValsOfTypeInVector( diff --git a/src/esp/core/Esp.h b/src/esp/core/Esp.h index 537e90c2cc..934372933a 100644 --- a/src/esp/core/Esp.h +++ b/src/esp/core/Esp.h @@ -98,6 +98,9 @@ constexpr int ID_UNDEFINED = -1; /** @brief Object ID of the rigid stage.*/ constexpr int RIGID_STAGE_ID = 0; +/** @brief Link ID of the baseLink for articulated objects */ +constexpr int BASELINK_ID = -1; + static const double NO_TIME = 0.0; /** diff --git a/src/esp/core/managedContainers/AbstractFileBasedManagedObject.h b/src/esp/core/managedContainers/AbstractFileBasedManagedObject.h index 402d8ee8ac..49d23239c6 100644 --- a/src/esp/core/managedContainers/AbstractFileBasedManagedObject.h +++ b/src/esp/core/managedContainers/AbstractFileBasedManagedObject.h @@ -42,6 +42,26 @@ class AbstractFileBasedManagedObject : public AbstractManagedObject { */ virtual std::string getActualFilename() const = 0; + /** + * @brief Get whether this ManagedObject has been saved to disk in its current + * state. Only applicable to registered ManagedObjects + */ + virtual bool isAttrSaved() const = 0; + + /** + * @brief Set that this ManagedObject has values that are different than its + * most recently saved-to-disk version. This is called when the ManagedObject + * is registered. + */ + + void setAttrIsNotSaved() { setFileSaveStatus(false); } + + /** + * @brief Set that this ManagedObject is the same as its saved-to-disk + * version. This is called when the ManagedObject is saved to disk. + */ + void setAttrIsSaved() { setFileSaveStatus(true); } + /** * @brief This will return a simplified version of the * AbstractFileBasedManagedObject handle, removing extensions and any parent @@ -66,6 +86,13 @@ class AbstractFileBasedManagedObject : public AbstractManagedObject { virtual io::JsonGenericValue writeToJsonObject( io::JsonAllocator& allocator) const = 0; + protected: + /** + * @brief Set this ManagedObject's save status (i.e. whether it matches its + * version on disk or not) + */ + virtual void setFileSaveStatus(bool _isSaved) = 0; + public: ESP_SMART_POINTERS(AbstractFileBasedManagedObject) }; // class AbstractFileBasedManagedObject diff --git a/src/esp/core/managedContainers/ManagedContainer.h b/src/esp/core/managedContainers/ManagedContainer.h index dd4590c011..4654c7939e 100644 --- a/src/esp/core/managedContainers/ManagedContainer.h +++ b/src/esp/core/managedContainers/ManagedContainer.h @@ -163,11 +163,10 @@ class ManagedContainer : public ManagedContainerBase { "registration, so registration aborted."; return ID_UNDEFINED; } - if ("" != objectHandle) { - return this->registerObjectInternal(std::move(managedObject), - objectHandle, forceRegistration); - } - std::string handleToSet = managedObject->getHandle(); + // If no handle give, query object for handle + std::string handleToSet = + ("" == objectHandle) ? managedObject->getHandle() : objectHandle; + // if still no handle, fail registration if ("" == handleToSet) { ESP_ERROR(Magnum::Debug::Flag::NoSpace) << "<" << this->objectType_ @@ -175,6 +174,7 @@ class ManagedContainer : public ManagedContainerBase { "so registration aborted."; return ID_UNDEFINED; } + // Perform actual registration return this->registerObjectInternal(std::move(managedObject), handleToSet, forceRegistration); } // ManagedContainer::registerObject @@ -193,8 +193,7 @@ class ManagedContainer : public ManagedContainerBase { */ ManagedPtr getObjectByID(int managedObjectID) const { std::string objectHandle = getObjectHandleByID(managedObjectID); - if (!checkExistsWithMessage(objectHandle, - "<" + this->objectType_ + ">::getObjectByID")) { + if (!checkExistsWithMessage(objectHandle, "getObjectByID")) { return nullptr; } return getObjectInternal(objectHandle); @@ -212,8 +211,7 @@ class ManagedContainer : public ManagedContainerBase { * exist */ ManagedPtr getObjectByHandle(const std::string& objectHandle) const { - if (!checkExistsWithMessage( - objectHandle, "<" + this->objectType_ + ">::getObjectByHandle")) { + if (!checkExistsWithMessage(objectHandle, "getObjectByHandle")) { return nullptr; } return getObjectInternal(objectHandle); @@ -250,7 +248,7 @@ class ManagedContainer : public ManagedContainerBase { /** * @brief Retrieve a map of key= std::string handle; value = copy of * ManagedPtr object where the handles match the passed @p . See @ref - * ManagedContainerBase::getObjectHandlesBySubStringPerType. + * ManagedContainerBase::getAllObjectHandlesBySubStringPerType. * @param subStr substring key to search for within existing managed objects. * @param contains whether to search for keys containing, or excluding, * passed @p subStr @@ -260,8 +258,8 @@ class ManagedContainer : public ManagedContainerBase { std::unordered_map getObjectsByHandleSubstring( const std::string& subStr = "", bool contains = true) { - std::vector keys = this->getObjectHandlesBySubStringPerType( - objectLibKeyByID_, subStr, contains, false); + std::vector keys = + this->getAllObjectHandlesBySubStringPerType(subStr, contains, false); std::unordered_map res; res.reserve(keys.size()); @@ -280,7 +278,7 @@ class ManagedContainer : public ManagedContainerBase { /** * @brief Templated version. Retrieve a map of key= std::string handle; value * = copy of ManagedPtr object where the handles match the passed @p . See - * @ref ManagedContainerBase::getObjectHandlesBySubStringPerType. + * @ref ManagedContainerBase::getAllObjectHandlesBySubStringPerType. * * @tparam Desired downcast class that inerheits from this ManagedContainer's * ManagedObject type. @@ -294,8 +292,11 @@ class ManagedContainer : public ManagedContainerBase { std::unordered_map> getObjectsByHandleSubstring(const std::string& subStr = "", bool contains = true) { - std::vector keys = this->getObjectHandlesBySubStringPerType( - objectLibKeyByID_, subStr, contains, false); + static_assert(std::is_base_of::value, + "ManagedContainer :: Desired type must be derived from " + "Managed object type"); + std::vector keys = + this->getAllObjectHandlesBySubStringPerType(subStr, contains, false); std::unordered_map> res; res.reserve(keys.size()); @@ -321,13 +322,10 @@ class ManagedContainer : public ManagedContainerBase { */ ManagedPtr removeObjectByID(int objectID) { std::string objectHandle = getObjectHandleByID(objectID); - if (!checkExistsWithMessage( - objectHandle, "<" + this->objectType_ + ">::removeObjectByID")) { + if (!checkExistsWithMessage(objectHandle, "removeObjectByID")) { return nullptr; } - return removeObjectInternal( - objectID, objectHandle, - "<" + this->objectType_ + ">::removeObjectByID"); + return removeObjectInternal(objectID, objectHandle, "removeObjectByID"); } /** @@ -339,17 +337,14 @@ class ManagedContainer : public ManagedContainerBase { * exist */ ManagedPtr removeObjectByHandle(const std::string& objectHandle) { - if (!checkExistsWithMessage(objectHandle, "<" + this->objectType_ + - ">::removeObjectByHandle")) { + if (!checkExistsWithMessage(objectHandle, "removeObjectByHandle")) { return nullptr; } int objectID = this->getObjectIDByHandle(objectHandle); if (objectID == ID_UNDEFINED) { return nullptr; } - return removeObjectInternal( - objectID, objectHandle, - "<" + this->objectType_ + ">::removeObjectByHandle"); + return removeObjectInternal(objectID, objectHandle, "removeObjectByHandle"); } /** @@ -461,6 +456,9 @@ class ManagedContainer : public ManagedContainerBase { */ template std::shared_ptr getObjectOrCopyByHandle(const std::string& objectHandle) { + static_assert(std::is_base_of::value, + "ManagedContainer :: Desired type must be derived from " + "Managed object type"); // call non-template version auto res = getObjectOrCopyByHandle(objectHandle); if (nullptr == res) { @@ -480,8 +478,7 @@ class ManagedContainer : public ManagedContainerBase { */ ManagedPtr getObjectCopyByID(int managedObjectID) { std::string objectHandle = getObjectHandleByID(managedObjectID); - if (!checkExistsWithMessage( - objectHandle, "<" + this->objectType_ + ">::getObjectCopyByID")) { + if (!checkExistsWithMessage(objectHandle, "getObjectCopyByID")) { return nullptr; } auto orig = getObjectInternal(objectHandle); @@ -496,8 +493,7 @@ class ManagedContainer : public ManagedContainerBase { * does not exist */ ManagedPtr getObjectCopyByHandle(const std::string& objectHandle) { - if (!checkExistsWithMessage(objectHandle, "<" + this->objectType_ + - ">::getObjectCopyByHandle")) { + if (!checkExistsWithMessage(objectHandle, "getObjectCopyByHandle")) { return nullptr; } auto orig = getObjectInternal(objectHandle); @@ -544,6 +540,9 @@ class ManagedContainer : public ManagedContainerBase { */ template std::shared_ptr getObjectCopyByID(int managedObjectID) { + static_assert(std::is_base_of::value, + "ManagedContainer :: Desired type must be derived from " + "Managed object type"); // call non-template version auto res = getObjectCopyByID(managedObjectID); if (nullptr == res) { @@ -563,6 +562,9 @@ class ManagedContainer : public ManagedContainerBase { */ template std::shared_ptr getObjectCopyByHandle(const std::string& objectHandle) { + static_assert(std::is_base_of::value, + "ManagedContainer :: Desired type must be derived from " + "Managed object type"); // call non-template version auto res = getObjectCopyByHandle(objectHandle); if (nullptr == res) { @@ -635,18 +637,22 @@ class ManagedContainer : public ManagedContainerBase { const std::string& src); /** - * @brief Build a shared pointer to a copy of a the passed managed object, - * of appropriate managed object type for passed object type. This is the - * function called by the copy constructor map. + * @brief This is the function called by the copy constructor map. Build a + * shared pointer to a copy of a the passed managed object, of appropriate + * managed object type for passed object type. + * * @tparam U Type of managed object being created - must be a derived class * of ManagedPtr * @param orig original object of type ManagedPtr being copied */ - template - ManagedPtr createObjectCopy(ManagedPtr& orig) { + template + ManagedPtr createObjCopyCtorMapEntry(ManagedPtr& orig) { + static_assert(std::is_base_of::value, + "ManagedContainer :: Desired type must be derived from " + "Managed object type"); // don't call init on copy - assume copy is already properly initialized. return U::create(*(static_cast(orig.get()))); - } // ManagedContainer:: + } // ManagedContainer::createObjCopyCtorMapEntry /** * @brief Build an @ref esp::core::managedContainers::AbstractManagedObject @@ -770,8 +776,7 @@ class ManagedContainer : public ManagedContainerBase { // original ManagedPtr managedObjectCopy = copyObject(object); // add to libraries - setObjectInternal(managedObjectCopy, objectHandle); - objectLibKeyByID_.emplace(objectID, objectHandle); + setObjectInternal(managedObjectCopy, objectID, objectHandle); return objectID; } // ManagedContainer::addObjectToLibrary @@ -817,8 +822,8 @@ auto ManagedContainer::removeObjectsBySubstring( getObjectHandlesBySubstring(subStr, contains); for (const std::string& objectHandle : handles) { int objID = this->getObjectIDByHandle(objectHandle); - ManagedPtr ptr = removeObjectInternal(objID, objectHandle, - "<" + this->objectType_ + ">"); + ManagedPtr ptr = + removeObjectInternal(objID, objectHandle, "removeObjectsBySubstring"); if (nullptr != ptr) { res.push_back(ptr); } @@ -832,19 +837,23 @@ auto ManagedContainer::removeObjectInternal( const std::string& objectHandle, const std::string& sourceStr) -> ManagedPtr { if (!checkExistsWithMessage(objectHandle, sourceStr)) { - ESP_DEBUG() << sourceStr << ": Unable to remove" << objectType_ - << "managed object" << objectHandle << ": Does not exist."; + ESP_DEBUG(Magnum::Debug::Flag::NoSpace) + << "<" + this->objectType_ + ">::" << sourceStr + << " : Unable to remove requested managed object `" << objectHandle + << "` : Does not exist."; return nullptr; } std::string msg; if (this->getIsUndeletable(objectHandle)) { msg = "Required Undeletable Managed Object"; } else if (this->getIsUserLocked(objectHandle)) { - msg = "User-locked Object. To delete managed object, unlock it"; + msg = "User-locked Object. To delete managed object, unlock it"; } if (msg.length() != 0) { - ESP_DEBUG() << sourceStr << ": Unable to remove" << objectType_ - << "managed object" << objectHandle << ":" << msg << "."; + ESP_DEBUG(Magnum::Debug::Flag::NoSpace) + << "<" + this->objectType_ + ">::" << sourceStr + << " : Unable to remove requested managed object `" << objectHandle + << "` : Object is a " << msg << "."; return nullptr; } ManagedPtr managedObject = getObjectInternal(objectHandle); diff --git a/src/esp/core/managedContainers/ManagedContainerBase.cpp b/src/esp/core/managedContainers/ManagedContainerBase.cpp index 099f2fc0ea..b7b4f25baf 100644 --- a/src/esp/core/managedContainers/ManagedContainerBase.cpp +++ b/src/esp/core/managedContainers/ManagedContainerBase.cpp @@ -15,8 +15,7 @@ namespace managedContainers { bool ManagedContainerBase::setLock(const std::string& objectHandle, bool lock) { // if managed object does not currently exist then do not attempt to modify // its lock state - if (!checkExistsWithMessage(objectHandle, - "<" + this->objectType_ + ">::setLock")) { + if (!checkExistsWithMessage(objectHandle, "setLock")) { return false; } // if setting lock else clearing lock diff --git a/src/esp/core/managedContainers/ManagedContainerBase.h b/src/esp/core/managedContainers/ManagedContainerBase.h index 6030f6f28a..aba0eb44be 100644 --- a/src/esp/core/managedContainers/ManagedContainerBase.h +++ b/src/esp/core/managedContainers/ManagedContainerBase.h @@ -92,7 +92,7 @@ class ManagedContainerBase { bool contains = true) { std::vector handles = getObjectHandlesBySubstring(subStr, contains); - return this->setLockByHandles(handles, lock); + return setLockByHandles(handles, lock); } // ManagedContainerBase::setLockBySubstring /** @@ -143,8 +143,8 @@ class ManagedContainerBase { * managed objects cannot be deleted, although they can be edited. */ std::vector getUndeletableObjectHandles() const { - std::vector res(this->undeletableObjectNames_.begin(), - this->undeletableObjectNames_.end()); + std::vector res(undeletableObjectNames_.begin(), + undeletableObjectNames_.end()); return res; } // ManagedContainerBase::getUndeletableObjectHandles @@ -154,7 +154,7 @@ class ManagedContainerBase { * @return True if handle exists and is undeletable. */ bool getIsUndeletable(const std::string& key) const { - return (this->undeletableObjectNames_.count(key) > 0); + return (undeletableObjectNames_.count(key) > 0); } /** @@ -164,8 +164,8 @@ class ManagedContainerBase { * locked. */ std::vector getUserLockedObjectHandles() const { - std::vector res(this->userLockedObjectNames_.begin(), - this->userLockedObjectNames_.end()); + std::vector res(userLockedObjectNames_.begin(), + userLockedObjectNames_.end()); return res; } // ManagedContainerBase::getUserLockedObjectHandles @@ -175,7 +175,7 @@ class ManagedContainerBase { * @return True if handle exists and is user-locked. */ bool getIsUserLocked(const std::string& key) const { - return (this->userLockedObjectNames_.count(key) > 0); + return (userLockedObjectNames_.count(key) > 0); } /** @@ -296,14 +296,16 @@ class ManagedContainerBase { } /** - * @brief Only used from class template AddObject method. put the passed + * @brief Only used from class template AddObject method. put the passed * smart poitner in the library. * @param ptr the smart pointer to the object being managed * @param handle the name (key) to use for the object in the library */ void setObjectInternal(const std::shared_ptr& ptr, + int objId, const std::string& handle) { objectLibrary_[handle] = ptr; + objectLibKeyByID_.emplace(objId, handle); } /** @@ -328,8 +330,9 @@ class ManagedContainerBase { const std::string& src) const { if (!getObjectLibHasHandle(objectHandle)) { ESP_ERROR(Magnum::Debug::Flag::NoSpace) - << src << ":" << objectType_ << " managed object handle `" - << objectHandle << "` not found in ManagedContainer, so aborting."; + << "<" + this->objectType_ + ">::" << src + << " : Managed object handle `" << objectHandle + << "` not found in ManagedContainer, so aborting."; return false; } return true; @@ -399,6 +402,26 @@ class ManagedContainerBase { bool contains, bool sorted) const; + /** + * @brief Get a list of all managed objects' handles of passed type whose + * origin handles contain substr, ignoring subStr's case. + * + * This version works on the internal objectLibKeyByID_ map + * @param subStr substring to search for within existing managed objects + * @param contains Whether to search for handles containing, or not + * containing, substr + * @param sorted whether the return vector values are sorted + * @return vector of 0 or more managed object handles containing/not + * containing the passed substring + */ + std::vector getAllObjectHandlesBySubStringPerType( + const std::string& subStr, + bool contains, + bool sorted) const { + return getObjectHandlesBySubStringPerType(objectLibKeyByID_, subStr, + contains, sorted); + } + /** * @brief Get a list of all managed objects' handles of passed type whose * origin handles contain substr, ignoring subStr's case. @@ -441,16 +464,40 @@ class ManagedContainerBase { virtual void resetFinalize() = 0; // ======== Instance Variables ======== - /** - * @brief Maps string keys to managed object managed objects - */ - std::unordered_map> objectLibrary_; /** @brief A descriptive name of the managed object being managed by this * manager. */ const std::string objectType_; + /** + * @brief Provide a const iterator over the @p objectLibrary_ + */ + std::pair< + std::unordered_map>::const_iterator, + std::unordered_map>::const_iterator> + getObjectLibIterator() const { + return std::make_pair(objectLibrary_.cbegin(), objectLibrary_.cend()); + } + + /** + * @brief Clear the mapping of undeletable object handles + */ + void clearUndeletableObjectNames() { undeletableObjectNames_.clear(); } + + /** + * @brief Add an undeleteable object's name to the mapping + */ + void addUndeletableObjectName(std::string objName) { + undeletableObjectNames_.insert(std::move(objName)); + } + + private: + /** + * @brief Maps string keys to managed object managed objects + */ + std::unordered_map> objectLibrary_; + /** * @brief Maps all object attribute IDs to the appropriate handles used * by lib diff --git a/src/esp/core/managedContainers/ManagedFileBasedContainer.h b/src/esp/core/managedContainers/ManagedFileBasedContainer.h index f7f0308766..d1bb7e701b 100644 --- a/src/esp/core/managedContainers/ManagedFileBasedContainer.h +++ b/src/esp/core/managedContainers/ManagedFileBasedContainer.h @@ -81,6 +81,9 @@ class ManagedFileBasedContainer : public ManagedContainer { const io::JsonGenericValue config = docConfig->GetObject(); ManagedFileIOPtr attr = this->buildManagedObjectFromDoc(filename, config); attr->setActualFilename(filename); + // Set attributes' status to saved (i.e. it matches the version on disk) + // since it was just loaded. + attr->setAttrIsSaved(); return this->postCreateRegister(std::move(attr), registerObject); } // ManagedFileBasedContainer::createObjectFromJSONFile @@ -110,6 +113,9 @@ class ManagedFileBasedContainer : public ManagedContainer { // convert doc to const value const io::JsonGenericValue config = docConfig->GetObject(); ManagedFileIOPtr attr = this->buildManagedObjectFromDoc(docName, config); + // Set attributes' status to saved (i.e. it matches the version on disk) + // since it was just built from an existing JSON string. + attr->setAttrIsSaved(); return this->postCreateRegister(std::move(attr), registerObject); } // ManagedFileBasedContainer::createObjectFromJSONString @@ -377,6 +383,8 @@ class ManagedFileBasedContainer : public ManagedContainer { // attributes. Note : this will not be "permanent" for the object unless // it is registered after this save. managedObject->setActualFilename(fullFilename); + // Set attributes' status to saved (i.e. it matches the version on disk) + managedObject->setAttrIsSaved(); } else { ESP_ERROR(Mn::Debug::Flag::NoSpace) << "<" << this->objectType_ << "> : Attempt to save to Filename `" diff --git a/src/esp/metadata/URDFParser.cpp b/src/esp/metadata/URDFParser.cpp index b7bc7f8091..46f17de59b 100644 --- a/src/esp/metadata/URDFParser.cpp +++ b/src/esp/metadata/URDFParser.cpp @@ -675,6 +675,16 @@ bool Parser::parseGeometry(Geometry& geom, const XMLElement* g) { return false; } else { parseVector3(geom.m_boxSize, shape->Attribute("size")); + if (geom.m_boxSize.min() == 0) { + ESP_ERROR() << "Collision box primitive with 0 scale detected " + << geom.m_boxSize + << ". Replacing zeros with 0.001 and continuing."; + for (int i = 0; i < 3; ++i) { + if (geom.m_boxSize[i] == 0) { + geom.m_boxSize[i] = 0.001; + } + } + } } } else if (type_name == "cylinder") { geom.m_type = GEOM_CYLINDER; diff --git a/src/esp/metadata/attributes/AbstractAttributes.cpp b/src/esp/metadata/attributes/AbstractAttributes.cpp index a1c3823101..6de0868cdc 100644 --- a/src/esp/metadata/attributes/AbstractAttributes.cpp +++ b/src/esp/metadata/attributes/AbstractAttributes.cpp @@ -22,6 +22,9 @@ AbstractAttributes::AbstractAttributes(const std::string& attributesClassKey, setHidden("__ID", 0); setHidden("__fileDirectory", ""); setHidden("__actualFilename", ""); + // Initialize attributes to be different than on version on disk, if one + // exists. This should be set to true on file load and on file save. + setHidden("__isAttrSaved", false); } } // namespace attributes diff --git a/src/esp/metadata/attributes/AbstractAttributes.h b/src/esp/metadata/attributes/AbstractAttributes.h index c51b28a555..ba414933d8 100644 --- a/src/esp/metadata/attributes/AbstractAttributes.h +++ b/src/esp/metadata/attributes/AbstractAttributes.h @@ -121,16 +121,36 @@ class AbstractAttributes return getSubconfigCopy("user_defined"); } + /** + * @brief Gets a const smart pointer reference to a view of the user-specified + * configuration data from config file. Habitat does not parse or process this + * data, but it will be available to the user via python bindings for each + * object. + */ + std::shared_ptr getUserConfigurationView() const { + return getSubconfigView("user_defined"); + } + /** * @brief Gets a smart pointer reference to the actual user-specified * configuration data from config file. Habitat does not parse or process this * data, but it will be available to the user via python bindings for each - * object. This method is for editing the configuration. + * object. This method is for editing the configuration. */ std::shared_ptr editUserConfiguration() { return editSubconfig("user_defined"); } + /** + * @brief Move an existing user_defined subconfiguration into this + * configuration, overwriting the existing copy if it exists. Habitat does not + * parse or process this data, but it will be available to the user via python + * bindings for each object. This method is for editing the configuration. + */ + void setUserConfiguration(std::shared_ptr& userAttr) { + setSubconfigPtr("user_defined", userAttr); + } + /** * @brief Returns the number of user-defined values (within the "user-defined" * sub-ConfigurationGroup) this attributes has. @@ -166,7 +186,42 @@ class AbstractAttributes getObjectInfoInternal()); } + /** + * @brief Check whether filepath-based fields have been set by user input + * but have not been verified to exist (such verification occurs when the + * attributes is registered.) + */ + bool getFilePathsAreDirty() const { return get("__fileNamesDirty"); } + + /** + * @brief Clear the flag that specifies that filepath-based fields have been + * set but not verfified to exist (such verification occurs when the + * attributes is registered.) + */ + void setFilePathsAreClean() { setHidden("__fileNamesDirty", false); } + + /** + * @brief Get whether this ManagedObject has been saved to disk in its current + * state. Only applicable to registered ManagedObjects + */ + bool isAttrSaved() const override { return get("__isAttrSaved"); } + protected: + /** + * @brief Set this ManagedObject's save status (i.e. whether it matches its + * version on disk or not) + */ + void setFileSaveStatus(bool _isSaved) override { + setHidden("__isAttrSaved", _isSaved); + } + + /** + * @brief Used internally only. Set the flag that specifies a filepath-based + * field has been set to some value but has not yet been verified to + * exist (such verification occurs when the attributes is registered.) + */ + void setFilePathsAreDirty() { setHidden("__fileNamesDirty", true); } + /** * @brief Changing access to setter so that Configuration bindings cannot be * used to set a reserved value to an incorrect type. The inheritors of this diff --git a/src/esp/metadata/attributes/AbstractObjectAttributes.cpp b/src/esp/metadata/attributes/AbstractObjectAttributes.cpp index 828ae95e3f..02debe8a6c 100644 --- a/src/esp/metadata/attributes/AbstractObjectAttributes.cpp +++ b/src/esp/metadata/attributes/AbstractObjectAttributes.cpp @@ -51,7 +51,7 @@ AbstractObjectAttributes::AbstractObjectAttributes( // This specifies that we want to investigate the state of the render and // collision handles before we allow this attributes to be registered. // Hidden field - setIsDirty(); + setFilePathsAreDirty(); // set up an existing subgroup for marker_sets attributes addOrEditSubgroup("marker_sets"); } // AbstractObjectAttributes ctor diff --git a/src/esp/metadata/attributes/AbstractObjectAttributes.h b/src/esp/metadata/attributes/AbstractObjectAttributes.h index f81b6351f5..be9258bb38 100644 --- a/src/esp/metadata/attributes/AbstractObjectAttributes.h +++ b/src/esp/metadata/attributes/AbstractObjectAttributes.h @@ -77,11 +77,18 @@ class AbstractObjectAttributes : public AbstractAttributes { double getUnitsToMeters() const { return get("units_to_meters"); } /** - * @brief If not visible can add dynamic non-rendered object into a scene. If - * is not visible then should not add object to drawables. + * @brief Set whether visible or not. If not visible can add dynamic + * non-rendered object into a scene. If is not visible then should not add + * object to drawables. */ void setIsVisible(bool isVisible) { set("is_visible", isVisible); } + /** + * @brief Get whether visible or not. If not visible can add dynamic + * non-rendered object into a scene. If is not visible then should not add + * object to drawables. + */ bool getIsVisible() const { return get("is_visible"); } + void setFrictionCoefficient(double frictionCoefficient) { set("friction_coefficient", frictionCoefficient); } @@ -114,7 +121,7 @@ class AbstractObjectAttributes : public AbstractAttributes { */ void setRenderAssetHandle(const std::string& renderAssetHandle) { set("render_asset", renderAssetHandle); - setIsDirty(); + setFilePathsAreDirty(); } /** @@ -133,7 +140,7 @@ class AbstractObjectAttributes : public AbstractAttributes { */ void setRenderAssetFullPath(const std::string& renderAssetHandle) { setHidden("__renderAssetFullPath", renderAssetHandle); - setIsDirty(); + setFilePathsAreDirty(); } /** @@ -272,7 +279,7 @@ class AbstractObjectAttributes : public AbstractAttributes { */ void setCollisionAssetHandle(const std::string& collisionAssetHandle) { set("collision_asset", collisionAssetHandle); - setIsDirty(); + setFilePathsAreDirty(); } /** @@ -291,7 +298,7 @@ class AbstractObjectAttributes : public AbstractAttributes { */ void setCollisionAssetFullPath(const std::string& collisionAssetHandle) { setHidden("__collisionAssetFullPath", collisionAssetHandle); - setIsDirty(); + setFilePathsAreDirty(); } /** @@ -501,9 +508,6 @@ class AbstractObjectAttributes : public AbstractAttributes { */ bool getForceFlatShading() const { return get("force_flat_shading"); } - bool getIsDirty() const { return get("__isDirty"); } - void setIsClean() { setHidden("__isDirty", false); } - /** * @brief Populate a json object with all the first-level values held in this * configuration. Default is overridden to handle special cases for @@ -587,7 +591,6 @@ class AbstractObjectAttributes : public AbstractAttributes { * @brief get AbstractObject specific info for csv string */ virtual std::string getAbstractObjectInfoInternal() const { return ""; }; - void setIsDirty() { setHidden("__isDirty", true); } public: ESP_SMART_POINTERS(AbstractObjectAttributes) diff --git a/src/esp/metadata/attributes/ArticulatedObjectAttributes.cpp b/src/esp/metadata/attributes/ArticulatedObjectAttributes.cpp index c54dd4832b..ba19c0b70c 100644 --- a/src/esp/metadata/attributes/ArticulatedObjectAttributes.cpp +++ b/src/esp/metadata/attributes/ArticulatedObjectAttributes.cpp @@ -16,6 +16,7 @@ ArticulatedObjectAttributes::ArticulatedObjectAttributes( init("render_asset", ""); init("semantic_id", 0); + init("is_visible", true); // Initialize the default base type to be free joint initTranslated("base_type", getAOBaseTypeName(ArticulatedObjectBaseType::Free)); @@ -41,7 +42,10 @@ ArticulatedObjectAttributes::ArticulatedObjectAttributes( // Initialize these so they exist in the configuration setHidden("__urdfFullPath", ""); setHidden("__renderAssetFullPath", ""); - + // This specifies that we want to investigate the state of the urdf and skin + // render asset handles before we allow this attributes to be registered. + // Hidden field + setFilePathsAreDirty(); // set up an existing subgroup for marker_sets attributes addOrEditSubgroup("marker_sets"); } // ArticulatedObjectAttributes ctor diff --git a/src/esp/metadata/attributes/ArticulatedObjectAttributes.h b/src/esp/metadata/attributes/ArticulatedObjectAttributes.h index 73da4fcbe3..87998b450d 100644 --- a/src/esp/metadata/attributes/ArticulatedObjectAttributes.h +++ b/src/esp/metadata/attributes/ArticulatedObjectAttributes.h @@ -53,6 +53,7 @@ class ArticulatedObjectAttributes : public AbstractAttributes { */ void setRenderAssetHandle(const std::string& renderAsset) { set("render_asset", renderAsset); + setFilePathsAreDirty(); } /** * @brief Gets the string name for the render asset relative path @@ -68,6 +69,7 @@ class ArticulatedObjectAttributes : public AbstractAttributes { */ void setRenderAssetFullPath(const std::string& renderAssetHandle) { setHidden("__renderAssetFullPath", renderAssetHandle); + setFilePathsAreDirty(); } /** @@ -99,6 +101,19 @@ class ArticulatedObjectAttributes : public AbstractAttributes { */ double getMassScale() const { return get("mass_scale"); } + /** + * @brief Set whether visible or not. If not visible can add dynamic + * non-rendered object into a scene. If is not visible then should not add + * object to drawables. + */ + void setIsVisible(bool isVisible) { set("is_visible", isVisible); } + /** + * @brief Get whether visible or not. If not visible can add dynamic + * non-rendered object into a scene. If is not visible then should not add + * object to drawables. + */ + bool getIsVisible() const { return get("is_visible"); } + /** * @brief Set the type of base/root joint to use to add this Articulated * Object to the world. Cannot be "UNSPECIFIED" diff --git a/src/esp/metadata/attributes/AttributesEnumMaps.cpp b/src/esp/metadata/attributes/AttributesEnumMaps.cpp index e585483104..1efdcbfe4d 100644 --- a/src/esp/metadata/attributes/AttributesEnumMaps.cpp +++ b/src/esp/metadata/attributes/AttributesEnumMaps.cpp @@ -206,6 +206,7 @@ std::string getShaderTypeName(ObjectInstanceShaderType shaderTypeVal) { const std::map InstanceTranslationOriginMap = { + {"default", SceneInstanceTranslationOrigin::Unknown}, {"asset_local", SceneInstanceTranslationOrigin::AssetLocal}, {"com", SceneInstanceTranslationOrigin::COM}, }; diff --git a/src/esp/metadata/attributes/PbrShaderAttributes.cpp b/src/esp/metadata/attributes/PbrShaderAttributes.cpp index cc2f1e4c8c..c063f75c58 100644 --- a/src/esp/metadata/attributes/PbrShaderAttributes.cpp +++ b/src/esp/metadata/attributes/PbrShaderAttributes.cpp @@ -28,16 +28,12 @@ PbrShaderAttributes::PbrShaderAttributes(const std::string& handle) // Default brdf lookup table is the brdflut from here: // https://github.com/SaschaWillems/Vulkan-glTF-PBR/blob/master/screenshots/tex_brdflut.png - // Setting the value directly so that it won't trigger the PbrIBLHelper handle - // creation. init("ibl_blut_filename", "brdflut_ldr_512x512.png"); - // Default equirectangular environment cube map - init("ibl_envmap_filename", "lythwood_room_1k.hdr"); - init("pbr_ibl_helper_key", - Cr::Utility::formatString("{}_{}", "brdflut_ldr_512x512", - "lythwood_room_1k.hdr")); + // Build the PbrIBLHelper key to check/retrive helpers in map in + // ResourceManager. + buildPbrShaderHelperKey("brdflut_ldr_512x512.png", "lythwood_room_1k.hdr"); init("tonemap_exposure", 4.5f); init("use_ibl_tonemap", true); diff --git a/src/esp/metadata/attributes/PbrShaderAttributes.h b/src/esp/metadata/attributes/PbrShaderAttributes.h index 2332c61925..252d852841 100644 --- a/src/esp/metadata/attributes/PbrShaderAttributes.h +++ b/src/esp/metadata/attributes/PbrShaderAttributes.h @@ -280,9 +280,8 @@ class PbrShaderAttributes : public AbstractAttributes { */ void setIBLBrdfLUTAssetHandle(const std::string& brdfLUTAsset) { set("ibl_blut_filename", brdfLUTAsset); - set("pbr_ibl_helper_key", - Cr::Utility::formatString("{}_{}", brdfLUTAsset, - get("ibl_envmap_filename"))); + buildPbrShaderHelperKey(brdfLUTAsset, + get("ibl_envmap_filename")); } /** * @brief Get the filename for the brdf lookup table used by the IBL @@ -299,9 +298,7 @@ class PbrShaderAttributes : public AbstractAttributes { */ void setIBLEnvMapAssetHandle(const std::string& envMapAsset) { set("ibl_envmap_filename", envMapAsset); - set("pbr_ibl_helper_key", - Cr::Utility::formatString( - "{}_{}", get("ibl_blut_filename"), envMapAsset)); + buildPbrShaderHelperKey(get("ibl_blut_filename"), envMapAsset); } /** @@ -319,7 +316,7 @@ class PbrShaderAttributes : public AbstractAttributes { * handle>'. */ std::string getPbrShaderHelperKey() const { - return get("pbr_ibl_helper_key"); + return get("__pbrIBLHelperKey"); } /** @@ -508,6 +505,17 @@ class PbrShaderAttributes : public AbstractAttributes { io::JsonAllocator& allocator) const override; protected: + /** + * @brief Used internally. Build the PbrShaderHelper Key from the ibl blut + * filename and the ibl envmap filename used to check/retrive helpers in map + * in ResourceManager. + */ + void buildPbrShaderHelperKey(const std::string& brdfLUTAsset, + const std::string& envMapAsset) { + setHidden("__pbrIBLHelperKey", + Cr::Utility::formatString("{}_{}", brdfLUTAsset, envMapAsset)); + } + /** * @brief Retrieve a comma-separated string holding the header values for the * info returned for this managed object, type-specific. diff --git a/src/esp/metadata/attributes/SceneInstanceAttributes.cpp b/src/esp/metadata/attributes/SceneInstanceAttributes.cpp index 48a1a97b07..254dded883 100644 --- a/src/esp/metadata/attributes/SceneInstanceAttributes.cpp +++ b/src/esp/metadata/attributes/SceneInstanceAttributes.cpp @@ -21,10 +21,12 @@ SceneObjectInstanceAttributes::SceneObjectInstanceAttributes( : AbstractAttributes(type, handle) { // default to unknown for object instances, to use attributes-specified // defaults - init("shader_type", getShaderTypeName(ObjectInstanceShaderType::Unspecified)); + initTranslated("shader_type", + getShaderTypeName(ObjectInstanceShaderType::Unspecified)); // defaults to unknown/undefined - init("motion_type", getMotionTypeName(esp::physics::MotionType::UNDEFINED)); + initTranslated("motion_type", + getMotionTypeName(esp::physics::MotionType::UNDEFINED)); // set to no rotation or translation init("rotation", Mn::Quaternion(Mn::Math::IdentityInit)); init("translation", Mn::Vector3()); @@ -32,8 +34,9 @@ SceneObjectInstanceAttributes::SceneObjectInstanceAttributes( // ID_UNDEFINED init("is_instance_visible", ID_UNDEFINED); // defaults to unknown so that obj instances use scene instance setting - init("translation_origin", - getTranslationOriginName(SceneInstanceTranslationOrigin::Unknown)); + initTranslated( + "translation_origin", + getTranslationOriginName(SceneInstanceTranslationOrigin::Unknown)); // set default multiplicative scaling values init("uniform_scale", 1.0); init("non_uniform_scale", Mn::Vector3{1.0, 1.0, 1.0}); @@ -50,18 +53,27 @@ SceneObjectInstanceAttributes::SceneObjectInstanceAttributes( // to a scene instance. // Handle is set via init in base class, which would not be written out to // file if we did not explicitly set it. + // NOTE : this will not call a virtual override + // (SceneAOInstanceAttributes::setHandle) of AbstractAttributes::setHandle due + // to virtual dispatch not being available in constructor setHandle(handle); // set appropriate fields from abstract object attributes // Not initialize, since these are not default values - set("shader_type", getShaderTypeName(baseObjAttribs->getShaderType())); - // set to match attributes setting - set("is_instance_visible", (baseObjAttribs->getIsVisible() ? 1 : 0)); + + // Need to verify that the baseObjAttribs values are not defaults before we + // set these values. + + if (!baseObjAttribs->isDefaultVal("shader_type")) { + setShaderType(getShaderTypeName(baseObjAttribs->getShaderType())); + } + if (!baseObjAttribs->isDefaultVal("is_visible")) { + setIsInstanceVisible(baseObjAttribs->getIsVisible()); + } // set nonuniform scale to match attributes scale - setNonUniformScale(baseObjAttribs->getScale()); - // Prepopulate user config to match baseObjAttribs' user config. - editUserConfiguration()->overwriteWithConfig( - baseObjAttribs->getUserConfiguration()); + if (!baseObjAttribs->isDefaultVal("scale")) { + setNonUniformScale(baseObjAttribs->getScale()); + } } std::string SceneObjectInstanceAttributes::getObjectInfoHeaderInternal() const { @@ -118,15 +130,10 @@ void SceneObjectInstanceAttributes::writeValuesToJson( io::JsonAllocator& allocator) const { // map "handle" to "template_name" key in json writeValueToJson("handle", "template_name", jsonObj, allocator); - if (getTranslation() != Mn::Vector3()) { - writeValueToJson("translation", jsonObj, allocator); - } - if (getTranslationOrigin() != SceneInstanceTranslationOrigin::Unknown) { - writeValueToJson("translation_origin", jsonObj, allocator); - } - if (getRotation() != Mn::Quaternion(Mn::Math::IdentityInit)) { - writeValueToJson("rotation", jsonObj, allocator); - } + writeValueToJson("translation", jsonObj, allocator); + writeValueToJson("translation_origin", jsonObj, allocator); + writeValueToJson("rotation", jsonObj, allocator); + // map "is_instance_visible" to boolean only if not -1, otherwise don't save int visSet = getIsInstanceVisible(); if (visSet != ID_UNDEFINED) { @@ -134,25 +141,12 @@ void SceneObjectInstanceAttributes::writeValuesToJson( auto jsonVal = io::toJsonValue(static_cast(visSet), allocator); jsonObj.AddMember("is_instance_visible", jsonVal, allocator); } - if (getMotionType() != esp::physics::MotionType::UNDEFINED) { - writeValueToJson("motion_type", jsonObj, allocator); - } - if (getShaderType() != ObjectInstanceShaderType::Unspecified) { - writeValueToJson("shader_type", jsonObj, allocator); - } - if (getUniformScale() != 1.0f) { - writeValueToJson("uniform_scale", jsonObj, allocator); - } - if (getNonUniformScale() != Mn::Vector3(1.0, 1.0, 1.0)) { - writeValueToJson("non_uniform_scale", jsonObj, allocator); - } - if (!getApplyScaleToMass()) { - writeValueToJson("apply_scale_to_mass", jsonObj, allocator); - } - if (getMassScale() != 1.0) { - writeValueToJson("mass_scale", jsonObj, allocator); - } - + writeValueToJson("motion_type", jsonObj, allocator); + writeValueToJson("shader_type", jsonObj, allocator); + writeValueToJson("uniform_scale", jsonObj, allocator); + writeValueToJson("non_uniform_scale", jsonObj, allocator); + writeValueToJson("apply_scale_to_mass", jsonObj, allocator); + writeValueToJson("mass_scale", jsonObj, allocator); // take care of child class values, if any exist writeValuesToJsonInternal(jsonObj, allocator); @@ -168,19 +162,21 @@ SceneAOInstanceAttributes::SceneAOInstanceAttributes(const std::string& handle) // Set the instance base type to be unspecified - if not set in instance json, // use ao_config value - init("base_type", getAOBaseTypeName(ArticulatedObjectBaseType::Unspecified)); + initTranslated("base_type", + getAOBaseTypeName(ArticulatedObjectBaseType::Unspecified)); // Set the instance source for the inertia calculation to be unspecified - if // not set in instance json, use ao_config value - init("inertia_source", - getAOInertiaSourceName(ArticulatedObjectInertiaSource::Unspecified)); + initTranslated( + "inertia_source", + getAOInertiaSourceName(ArticulatedObjectInertiaSource::Unspecified)); // Set the instance link order to use as unspecified - if not set in instance // json, use ao_config value - init("link_order", - getAOLinkOrderName(ArticulatedObjectLinkOrder::Unspecified)); + initTranslated("link_order", + getAOLinkOrderName(ArticulatedObjectLinkOrder::Unspecified)); // Set render mode to be unspecified - if not set in instance json, use // ao_config value - init("render_mode", - getAORenderModeName(ArticulatedObjectRenderMode::Unspecified)); + initTranslated("render_mode", + getAORenderModeName(ArticulatedObjectRenderMode::Unspecified)); editSubconfig("initial_joint_pose"); editSubconfig("initial_joint_velocities"); } @@ -194,27 +190,40 @@ SceneAOInstanceAttributes::SceneAOInstanceAttributes( // a scene instance. // Handle is set via init in base class, which would not be written out to // file if we did not explicitly set it. + // NOTE : this will not call a virtual override + // (SceneAOInstanceAttributes::setHandle) of AbstractAttributes::setHandle due + // to virtual dispatch not being available in constructor setHandle(handle); // Should not initialize these values but set them, since these are not // default values, but from an existing AO attributes. + + // Need to verify that the aObjAttribs values are not defaults before we + // set these values. // Set shader type to use aObjAttribs value - setShaderType(getShaderTypeName(aObjAttribs->getShaderType())); - // Set the instance base type to use aObjAttribs value - setBaseType(getAOBaseTypeName(aObjAttribs->getBaseType())); - // Set the instance source for the inertia calculation to use aObjAttribs - // value - setInertiaSource(getAOInertiaSourceName(aObjAttribs->getInertiaSource())); - // Set the instance link order to use aObjAttribs value - setLinkOrder(getAOLinkOrderName(aObjAttribs->getLinkOrder())); - // Set render mode to use aObjAttribs value - setRenderMode(getAORenderModeName(aObjAttribs->getRenderMode())); - - // Prepopulate user config to match attribs' user config. - editUserConfiguration()->overwriteWithConfig( - aObjAttribs->getUserConfiguration()); - editSubconfig("initial_joint_pose"); - editSubconfig("initial_joint_velocities"); + if (!aObjAttribs->isDefaultVal("shader_type")) { + setShaderType(getShaderTypeName(aObjAttribs->getShaderType())); + } + if (!aObjAttribs->isDefaultVal("is_visible")) { + setIsInstanceVisible(aObjAttribs->getIsVisible()); + } + if (!aObjAttribs->isDefaultVal("base_type")) { + // Set the instance base type to use aObjAttribs value + setBaseType(getAOBaseTypeName(aObjAttribs->getBaseType())); + } + if (!aObjAttribs->isDefaultVal("inertia_source")) { + // Set the instance source for the inertia calculation to use aObjAttribs + // value + setInertiaSource(getAOInertiaSourceName(aObjAttribs->getInertiaSource())); + } + if (!aObjAttribs->isDefaultVal("link_order")) { + // Set the instance link order to use aObjAttribs value + setLinkOrder(getAOLinkOrderName(aObjAttribs->getLinkOrder())); + } + if (!aObjAttribs->isDefaultVal("render_mode")) { + // Set render mode to use aObjAttribs value + setRenderMode(getAORenderModeName(aObjAttribs->getRenderMode())); + } } std::string SceneAOInstanceAttributes::getSceneObjInstanceInfoHeaderInternal() @@ -262,19 +271,10 @@ std::string SceneAOInstanceAttributes::getSceneObjInstanceInfoInternal() const { void SceneAOInstanceAttributes::writeValuesToJsonInternal( io::JsonGenericValue& jsonObj, io::JsonAllocator& allocator) const { - if (getBaseType() != ArticulatedObjectBaseType::Unspecified) { - writeValueToJson("base_type", jsonObj, allocator); - } - if (getInertiaSource() != ArticulatedObjectInertiaSource::Unspecified) { - writeValueToJson("inertia_source", jsonObj, allocator); - } - if (getLinkOrder() != ArticulatedObjectLinkOrder::Unspecified) { - writeValueToJson("link_order", jsonObj, allocator); - } - if (getRenderMode() != ArticulatedObjectRenderMode::Unspecified) { - writeValueToJson("render_mode", jsonObj, allocator); - } - + writeValueToJson("base_type", jsonObj, allocator); + writeValueToJson("inertia_source", jsonObj, allocator); + writeValueToJson("link_order", jsonObj, allocator); + writeValueToJson("render_mode", jsonObj, allocator); writeValueToJson("auto_clamp_joint_limits", jsonObj, allocator); } // SceneAOInstanceAttributes::writeValuesToJsonInternal diff --git a/src/esp/metadata/attributes/SceneInstanceAttributes.h b/src/esp/metadata/attributes/SceneInstanceAttributes.h index 9c6d95d02d..49807b762a 100644 --- a/src/esp/metadata/attributes/SceneInstanceAttributes.h +++ b/src/esp/metadata/attributes/SceneInstanceAttributes.h @@ -73,7 +73,7 @@ class SceneObjectInstanceAttributes : public AbstractAttributes { << translation_origin << "attempted to be set in SceneObjectInstanceAttributes :" << getHandle() << ". Aborting."); - set("translation_origin", translation_origin); + setTranslated("translation_origin", translation_origin); } /** @@ -93,6 +93,13 @@ class SceneObjectInstanceAttributes : public AbstractAttributes { return SceneInstanceTranslationOrigin::Unknown; } + /** + * @brief Get string representation of translation origin + */ + std::string getTranslationOriginStr() const { + return get("translation_origin"); + } + /** * @brief Set the rotation of the object */ @@ -145,7 +152,7 @@ class SceneObjectInstanceAttributes : public AbstractAttributes { << "attempted to be set in SceneObjectInstanceAttributes :" << getHandle() << ". Aborting."); - set("shader_type", shader_type); + setTranslated("shader_type", shader_type); } /** @@ -324,7 +331,7 @@ class SceneAOInstanceAttributes : public SceneObjectInstanceAttributes { << baseType << "attempted to be set in ArticulatedObjectAttributes:" << getHandle() << ". Aborting."); - set("base_type", baseType); + setTranslated("base_type", baseType); } /** @@ -356,7 +363,7 @@ class SceneAOInstanceAttributes : public SceneObjectInstanceAttributes { << inertiaSrc << "attempted to be set in ArticulatedObjectAttributes:" << getHandle() << ". Aborting."); - set("inertia_source", inertiaSrc); + setTranslated("inertia_source", inertiaSrc); } /** @@ -388,7 +395,7 @@ class SceneAOInstanceAttributes : public SceneObjectInstanceAttributes { << linkOrder << "attempted to be set in ArticulatedObjectAttributes:" << getHandle() << ". Aborting."); - set("link_order", linkOrder); + setTranslated("link_order", linkOrder); } /** @@ -419,7 +426,7 @@ class SceneAOInstanceAttributes : public SceneObjectInstanceAttributes { << renderMode << "attempted to be set in ArticulatedObjectAttributes:" << getHandle() << ". Aborting."); - set("render_mode", renderMode); + setTranslated("render_mode", renderMode); } /** diff --git a/src/esp/metadata/attributes/StageAttributes.h b/src/esp/metadata/attributes/StageAttributes.h index 7c7fbe9a24..e800fa28a4 100644 --- a/src/esp/metadata/attributes/StageAttributes.h +++ b/src/esp/metadata/attributes/StageAttributes.h @@ -82,7 +82,7 @@ class StageAttributes : public AbstractObjectAttributes { void setSemanticDescriptorFilename( const std::string& semantic_descriptor_filename) { set("semantic_descriptor_filename", semantic_descriptor_filename); - setIsDirty(); + setFilePathsAreDirty(); } /** * @brief Text file that describes the hierharchy of semantic information @@ -101,7 +101,7 @@ class StageAttributes : public AbstractObjectAttributes { void setSemanticDescriptorFullPath( const std::string& semanticDescriptorHandle) { setHidden("__semanticDescriptorFullPath", semanticDescriptorHandle); - setIsDirty(); + setFilePathsAreDirty(); } /** @@ -121,7 +121,7 @@ class StageAttributes : public AbstractObjectAttributes { */ void setSemanticAssetHandle(const std::string& semanticAssetHandle) { set("semantic_asset", semanticAssetHandle); - setIsDirty(); + setFilePathsAreDirty(); } /** @@ -141,7 +141,7 @@ class StageAttributes : public AbstractObjectAttributes { */ void setSemanticAssetFullPath(const std::string& semanticAssetHandle) { setHidden("__semanticAssetFullPath", semanticAssetHandle); - setIsDirty(); + setFilePathsAreDirty(); } /** @@ -298,7 +298,7 @@ class StageAttributes : public AbstractObjectAttributes { void setNavmeshAssetHandle(const std::string& nav_asset) { set("nav_asset", nav_asset); - setIsDirty(); + setFilePathsAreDirty(); } std::string getNavmeshAssetHandle() const { return get("nav_asset"); @@ -311,7 +311,7 @@ class StageAttributes : public AbstractObjectAttributes { */ void setNavmeshAssetFullPath(const std::string& navmeshAssetHandle) { setHidden("__navmeshAssetFullPath", navmeshAssetHandle); - setIsDirty(); + setFilePathsAreDirty(); } /** diff --git a/src/esp/metadata/managers/AOAttributesManager.cpp b/src/esp/metadata/managers/AOAttributesManager.cpp index 1b26db54ca..5b047ef636 100644 --- a/src/esp/metadata/managers/AOAttributesManager.cpp +++ b/src/esp/metadata/managers/AOAttributesManager.cpp @@ -254,6 +254,8 @@ AOAttributesManager::preRegisterObjectFinalize( // filter all paths properly so that the handles don't have filepaths and the // accessors are hidden fields this->finalizeAttrPathsBeforeRegister(AOAttributesTemplate); + // Clear dirty flag from when asset handles are changed + AOAttributesTemplate->setFilePathsAreClean(); return core::managedContainers::ManagedObjectPreregistration::Success; } // AOAttributesManager::preRegisterObjectFinalize @@ -289,8 +291,11 @@ void AOAttributesManager::finalizeAttrPathsBeforeRegister( std::map AOAttributesManager::getArticulatedObjectModelFilenames() const { std::map articulatedObjPaths; - for (const auto& val : this->objectLibrary_) { - auto attr = this->getObjectByHandle(val.first); + + auto objIterPair = this->getObjectLibIterator(); + for (auto& objIter = objIterPair.first; objIter != objIterPair.second; + ++objIter) { + auto attr = this->getObjectByHandle(objIter->first); auto key = attr->getSimplifiedHandle(); auto urdf = attr->getURDFFullPath(); articulatedObjPaths[key] = urdf; diff --git a/src/esp/metadata/managers/AOAttributesManager.h b/src/esp/metadata/managers/AOAttributesManager.h index c415e133d2..b545718d75 100644 --- a/src/esp/metadata/managers/AOAttributesManager.h +++ b/src/esp/metadata/managers/AOAttributesManager.h @@ -28,7 +28,7 @@ class AOAttributesManager ManagedObjectAccess::Copy>:: AbstractAttributesManager("Articulated Object", "ao_config.json") { this->copyConstructorMap_["ArticulatedObjectAttributes"] = - &AOAttributesManager::createObjectCopy< + &AOAttributesManager::createObjCopyCtorMapEntry< attributes::ArticulatedObjectAttributes>; } // ctor @@ -43,7 +43,7 @@ class AOAttributesManager * overwritten with the newly created one if registerTemplate is true. * * @param aoConfigFilename The configuration file to parse. - * @param registerTemplate whether to add this template to the library. + * @param registerTemplate Whether to add this template to the library. * If the user is going to edit this template, this should be false - any * subsequent editing will require re-registration. Defaults to true. If * specified as true, then this function returns a copy of the registered diff --git a/src/esp/metadata/managers/AbstractAttributesManager.h b/src/esp/metadata/managers/AbstractAttributesManager.h index 5274f0c0ea..2ecd33b76f 100644 --- a/src/esp/metadata/managers/AbstractAttributesManager.h +++ b/src/esp/metadata/managers/AbstractAttributesManager.h @@ -425,7 +425,7 @@ AbstractAttributesManager::loadAllFileBasedTemplates( // save handles in list of defaults, so they are not removed, if desired. if (saveAsDefaults) { std::string tmpltHandle = tmplt->getHandle(); - this->undeletableObjectNames_.insert(std::move(tmpltHandle)); + this->addUndeletableObjectName(std::move(tmpltHandle)); } templateIndices[i] = tmplt->getID(); } diff --git a/src/esp/metadata/managers/AssetAttributesManager.cpp b/src/esp/metadata/managers/AssetAttributesManager.cpp index a5f08b0b67..0302915ef4 100644 --- a/src/esp/metadata/managers/AssetAttributesManager.cpp +++ b/src/esp/metadata/managers/AssetAttributesManager.cpp @@ -83,19 +83,25 @@ AssetAttributesManager::AssetAttributesManager() // function pointers to asset attributes copy constructors this->copyConstructorMap_["CapsulePrimitiveAttributes"] = - &AssetAttributesManager::createObjectCopy; + &AssetAttributesManager::createObjCopyCtorMapEntry< + CapsulePrimitiveAttributes>; this->copyConstructorMap_["ConePrimitiveAttributes"] = - &AssetAttributesManager::createObjectCopy; + &AssetAttributesManager::createObjCopyCtorMapEntry< + ConePrimitiveAttributes>; this->copyConstructorMap_["CubePrimitiveAttributes"] = - &AssetAttributesManager::createObjectCopy; + &AssetAttributesManager::createObjCopyCtorMapEntry< + CubePrimitiveAttributes>; this->copyConstructorMap_["CylinderPrimitiveAttributes"] = - &AssetAttributesManager::createObjectCopy; + &AssetAttributesManager::createObjCopyCtorMapEntry< + CylinderPrimitiveAttributes>; this->copyConstructorMap_["IcospherePrimitiveAttributes"] = - &AssetAttributesManager::createObjectCopy; + &AssetAttributesManager::createObjCopyCtorMapEntry< + IcospherePrimitiveAttributes>; this->copyConstructorMap_["UVSpherePrimitiveAttributes"] = - &AssetAttributesManager::createObjectCopy; + &AssetAttributesManager::createObjCopyCtorMapEntry< + UVSpherePrimitiveAttributes>; // no entry added for PrimObjTypes::END_PRIM_OBJ_TYPES - this->undeletableObjectNames_.clear(); + this->clearUndeletableObjectNames(); // build default AbstractPrimitiveAttributes objects for (const std::pair& elem : PrimitiveNames3DMap) { @@ -105,7 +111,7 @@ AssetAttributesManager::AssetAttributesManager() auto tmplt = AssetAttributesManager::createObject(elem.second, true); std::string tmpltHandle = tmplt->getHandle(); defaultPrimAttributeHandles_[elem.second] = tmpltHandle; - this->undeletableObjectNames_.insert(std::move(tmpltHandle)); + this->addUndeletableObjectName(std::move(tmpltHandle)); } ESP_DEBUG() << "Built default primitive asset templates :" diff --git a/src/esp/metadata/managers/AssetAttributesManager.h b/src/esp/metadata/managers/AssetAttributesManager.h index 3c084061e4..75bd106828 100644 --- a/src/esp/metadata/managers/AssetAttributesManager.h +++ b/src/esp/metadata/managers/AssetAttributesManager.h @@ -224,8 +224,7 @@ class AssetAttributesManager return {}; } std::string subStr = PrimitiveNames3DMap.at(primType); - return this->getObjectHandlesBySubStringPerType(this->objectLibKeyByID_, - subStr, contains, true); + return this->getAllObjectHandlesBySubStringPerType(subStr, contains, true); } // AssetAttributeManager::getTemplateHandlesByPrimType /** diff --git a/src/esp/metadata/managers/LightLayoutAttributesManager.h b/src/esp/metadata/managers/LightLayoutAttributesManager.h index 5a2fcc4b9b..ef3ca652ff 100644 --- a/src/esp/metadata/managers/LightLayoutAttributesManager.h +++ b/src/esp/metadata/managers/LightLayoutAttributesManager.h @@ -27,7 +27,7 @@ class LightLayoutAttributesManager "lighting_config.json") { // build this manager's copy constructor map this->copyConstructorMap_["LightLayoutAttributes"] = - &LightLayoutAttributesManager::createObjectCopy< + &LightLayoutAttributesManager::createObjCopyCtorMapEntry< attributes::LightLayoutAttributes>; } @@ -82,7 +82,9 @@ class LightLayoutAttributesManager const std::string& lightConfigName); /** - * @brief This function will be called to finalize attributes' paths before + * @brief Not required for this manager. + * + * This function will be called to finalize attributes' paths before * registration, moving fully qualified paths to the appropriate hidden * attribute fields. This can also be called without registration to make sure * the paths specified in an attributes are properly configured. diff --git a/src/esp/metadata/managers/ObjectAttributesManager.cpp b/src/esp/metadata/managers/ObjectAttributesManager.cpp index c193e64d65..fc374553bd 100644 --- a/src/esp/metadata/managers/ObjectAttributesManager.cpp +++ b/src/esp/metadata/managers/ObjectAttributesManager.cpp @@ -67,7 +67,7 @@ void ObjectAttributesManager::createDefaultPrimBasedAttributesTemplates() { auto tmplt = createPrimBasedAttributesTemplate(elem, true); // save handles in list of defaults, so they are not removed std::string tmpltHandle = tmplt->getHandle(); - this->undeletableObjectNames_.insert(std::move(tmpltHandle)); + this->addUndeletableObjectName(std::move(tmpltHandle)); } } // ObjectAttributesManager::createDefaultPrimBasedAttributesTemplates @@ -301,7 +301,7 @@ ObjectAttributesManager::preRegisterObjectFinalize( // accessors are hidden fields this->finalizeAttrPathsBeforeRegister(objectTemplate); // Clear dirty flag from when asset handles are changed - objectTemplate->setIsClean(); + objectTemplate->setFilePathsAreClean(); return core::managedContainers::ManagedObjectPreregistration::Success; } // ObjectAttributesManager::preRegisterObjectFinalize diff --git a/src/esp/metadata/managers/ObjectAttributesManager.h b/src/esp/metadata/managers/ObjectAttributesManager.h index d68d73ff98..de49f52ee7 100644 --- a/src/esp/metadata/managers/ObjectAttributesManager.h +++ b/src/esp/metadata/managers/ObjectAttributesManager.h @@ -29,7 +29,7 @@ class ObjectAttributesManager AbstractObjectAttributesManager("Object", "object_config.json") { // build this manager's copy constructor map this->copyConstructorMap_["ObjectAttributes"] = - &ObjectAttributesManager::createObjectCopy< + &ObjectAttributesManager::createObjCopyCtorMapEntry< attributes::ObjectAttributes>; } diff --git a/src/esp/metadata/managers/PbrShaderAttributesManager.cpp b/src/esp/metadata/managers/PbrShaderAttributesManager.cpp index 47e36f8edb..8bbcf939ec 100644 --- a/src/esp/metadata/managers/PbrShaderAttributesManager.cpp +++ b/src/esp/metadata/managers/PbrShaderAttributesManager.cpp @@ -23,7 +23,7 @@ PbrShaderAttributes::ptr PbrShaderAttributesManager::createObject( pbrConfigFilename, msg, registerTemplate); if (nullptr != attrs) { - ESP_DEBUG() << msg << "pbr shader configuration created" + ESP_DEBUG() << msg << "PBR Shader Attributes created" << (registerTemplate ? "and registered." : "."); } return attrs; @@ -218,7 +218,24 @@ void PbrShaderAttributesManager::setValsFromJSONDoc( // check for user defined attributes this->parseUserDefinedJsonVals(pbrShaderAttribs, jsonConfig); -} // PbrShaderAttributesManager::createFileBasedAttributesTemplate +} // PbrShaderAttributesManager::setValsFromJSONDoc + +core::managedContainers::ManagedObjectPreregistration +PbrShaderAttributesManager::preRegisterObjectFinalize( + attributes::PbrShaderAttributes::ptr pbrShaderAttribs, + const std::string& /*objectHandle*/, + bool /*forceRegistration*/) { + // TODO : Verify filenames exist as files or as resources + this->finalizeAttrPathsBeforeRegister(pbrShaderAttribs); + return core::managedContainers::ManagedObjectPreregistration::Success; +} // PbrShaderAttributesManager::preRegisterObjectFinalize + +void PbrShaderAttributesManager::finalizeAttrPathsBeforeRegister( + const attributes::PbrShaderAttributes::ptr& attributes) const { + // TODO Verify getIBLBrdfLUTAssetHandle and getIBLEnvMapAssetHandle exist as + // either file-based assets or resources and build paths to be relative if + // file-based +} // PbrShaderAttributesManager::finalizeAttrPathsBeforeRegister PbrShaderAttributes::ptr PbrShaderAttributesManager::initNewObjectInternal( const std::string& handleName, diff --git a/src/esp/metadata/managers/PbrShaderAttributesManager.h b/src/esp/metadata/managers/PbrShaderAttributesManager.h index ec075011dc..2512bc8e2b 100644 --- a/src/esp/metadata/managers/PbrShaderAttributesManager.h +++ b/src/esp/metadata/managers/PbrShaderAttributesManager.h @@ -27,7 +27,7 @@ class PbrShaderAttributesManager ManagedObjectAccess::Copy>:: AbstractAttributesManager("PBR Rendering", "pbr_config.json") { this->copyConstructorMap_["PbrShaderAttributes"] = - &PbrShaderAttributesManager::createObjectCopy< + &PbrShaderAttributesManager::createObjCopyCtorMapEntry< attributes::PbrShaderAttributes>; } // ctor @@ -70,11 +70,14 @@ class PbrShaderAttributesManager * to have IBL either on or off. */ void setAllIBLEnabled(bool isIblEnabled) { - for (const auto& val : this->objectLibrary_) { + auto objIterPair = this->getObjectLibIterator(); + for (auto& objIter = objIterPair.first; objIter != objIterPair.second; + ++objIter) { + const std::string objHandle = objIter->first; // Don't change system default - if (val.first.find(ESP_DEFAULT_PBRSHADER_CONFIG_REL_PATH) == + if (objHandle.find(ESP_DEFAULT_PBRSHADER_CONFIG_REL_PATH) == std::string::npos) { - this->getObjectByHandle(val.first)->setEnableIBL(isIblEnabled); + this->getObjectByHandle(objHandle)->setEnableIBL(isIblEnabled); } } } // PbrShaderAttributesManager::setAllIBLEnabled @@ -84,11 +87,14 @@ class PbrShaderAttributesManager * to have Direct Ligthing either on or off. */ void setAllDirectLightsEnabled(bool isDirLightEnabled) { - for (const auto& val : this->objectLibrary_) { + auto objIterPair = this->getObjectLibIterator(); + for (auto& objIter = objIterPair.first; objIter != objIterPair.second; + ++objIter) { + const std::string objHandle = objIter->first; // Don't change system default - if (val.first.find(ESP_DEFAULT_PBRSHADER_CONFIG_REL_PATH) == + if (objHandle.find(ESP_DEFAULT_PBRSHADER_CONFIG_REL_PATH) == std::string::npos) { - this->getObjectByHandle(val.first)->setEnableDirectLighting( + this->getObjectByHandle(objHandle)->setEnableDirectLighting( isDirLightEnabled); } } @@ -102,11 +108,11 @@ class PbrShaderAttributesManager * * TODO : If/When we begin treating IBL filepaths like we do other paths, this * will need to be implemented. - * @param attributes The attributes to be filtered. + * @param pbrShaderAttribs The attributes to be filtered. */ void finalizeAttrPathsBeforeRegister( - CORRADE_UNUSED const attributes::PbrShaderAttributes::ptr& attributes) - const override{}; + CORRADE_UNUSED const attributes::PbrShaderAttributes::ptr& + pbrShaderAttribs) const override; protected: /** @@ -139,12 +145,10 @@ class PbrShaderAttributesManager CORRADE_UNUSED const std::string& templateHandle) override {} /** - * @brief Not required for this manager. - * - * This method will perform any essential updating to the managed object - * before registration is performed. If this updating fails, registration will - * also fail. - * @param object the managed object to be registered + * @brief This method will perform any essential updating to the managed + * object before registration is performed. If this updating fails, + * registration will also fail. + * @param pbrShaderAttribs the managed object to be registered * @param objectHandle the name to register the managed object with. * Expected to be valid. * @param forceRegistration Should register object even if conditional @@ -154,12 +158,9 @@ class PbrShaderAttributesManager */ core::managedContainers::ManagedObjectPreregistration preRegisterObjectFinalize( - CORRADE_UNUSED attributes::PbrShaderAttributes::ptr object, + attributes::PbrShaderAttributes::ptr pbrShaderAttribs, CORRADE_UNUSED const std::string& objectHandle, - CORRADE_UNUSED bool forceRegistration) override { - // No pre-registration conditioning performed - return core::managedContainers::ManagedObjectPreregistration::Success; - } + CORRADE_UNUSED bool forceRegistration) override; /** * @brief Not required for this manager. diff --git a/src/esp/metadata/managers/PhysicsAttributesManager.h b/src/esp/metadata/managers/PhysicsAttributesManager.h index 524cc74049..e418fcee84 100644 --- a/src/esp/metadata/managers/PhysicsAttributesManager.h +++ b/src/esp/metadata/managers/PhysicsAttributesManager.h @@ -28,7 +28,7 @@ class PhysicsAttributesManager AbstractAttributesManager("Physics Manager", "physics_config.json") { this->copyConstructorMap_["PhysicsManagerAttributes"] = - &PhysicsAttributesManager::createObjectCopy< + &PhysicsAttributesManager::createObjCopyCtorMapEntry< attributes::PhysicsManagerAttributes>; } // ctor diff --git a/src/esp/metadata/managers/SceneDatasetAttributesManager.cpp b/src/esp/metadata/managers/SceneDatasetAttributesManager.cpp index ea800d7427..5d2932804c 100644 --- a/src/esp/metadata/managers/SceneDatasetAttributesManager.cpp +++ b/src/esp/metadata/managers/SceneDatasetAttributesManager.cpp @@ -25,7 +25,7 @@ SceneDatasetAttributesManager::SceneDatasetAttributesManager( pbrShaderAttributesManager_(std::move(pbrShaderAttributesMgr)) { // build this manager's copy ctor map this->copyConstructorMap_["SceneDatasetAttributes"] = - &SceneDatasetAttributesManager::createObjectCopy< + &SceneDatasetAttributesManager::createObjCopyCtorMapEntry< attributes::SceneDatasetAttributes>; } // SceneDatasetAttributesManager ctor diff --git a/src/esp/metadata/managers/SceneDatasetAttributesManager.h b/src/esp/metadata/managers/SceneDatasetAttributesManager.h index 59c1136b96..d95ca497de 100644 --- a/src/esp/metadata/managers/SceneDatasetAttributesManager.h +++ b/src/esp/metadata/managers/SceneDatasetAttributesManager.h @@ -67,8 +67,10 @@ class SceneDatasetAttributesManager */ void setCurrPhysicsManagerAttributesHandle(const std::string& handle) { physicsManagerAttributesHandle_ = handle; - for (const auto& val : this->objectLibrary_) { - this->getObjectByHandle(val.first)->setPhysicsManagerHandle(handle); + auto objIterPair = this->getObjectLibIterator(); + for (auto& objIter = objIterPair.first; objIter != objIterPair.second; + ++objIter) { + this->getObjectByHandle(objIter->first)->setPhysicsManagerHandle(handle); } } // SceneDatasetAttributesManager::setCurrPhysicsManagerAttributesHandle @@ -87,9 +89,11 @@ class SceneDatasetAttributesManager */ void setDefaultPbrShaderAttributesHandle(const std::string& pbrHandle) { defaultPbrShaderAttributesHandle_ = pbrHandle; - for (const auto& val : this->objectLibrary_) { - this->getObjectByHandle(val.first)->setDefaultPbrShaderAttrHandle( - pbrHandle); + auto objIterPair = this->getObjectLibIterator(); + for (auto& objIter = objIterPair.first; objIter != objIterPair.second; + ++objIter) { + this->getObjectByHandle(objIter->first) + ->setDefaultPbrShaderAttrHandle(pbrHandle); } } // SceneDatasetAttributesManager::setDefaultPbrShaderAttributesHandle diff --git a/src/esp/metadata/managers/SceneInstanceAttributesManager.cpp b/src/esp/metadata/managers/SceneInstanceAttributesManager.cpp index 5d8574dbf1..0494f951eb 100644 --- a/src/esp/metadata/managers/SceneInstanceAttributesManager.cpp +++ b/src/esp/metadata/managers/SceneInstanceAttributesManager.cpp @@ -375,9 +375,12 @@ void SceneInstanceAttributesManager::setAbstractObjectAttributesFromJson( instanceAttrs->setHandle(template_name); }); - // Check for translation origin override for a particular instance. Default + // Check for translation origin override for a particular instance. Default // to unknown, which will mean use scene instance-level default. - instanceAttrs->setTranslationOrigin(getTranslationOriginVal(jCell)); + const std::string transOriginStr = getTranslationOriginVal(jCell); + if (transOriginStr != instanceAttrs->getTranslationOriginStr()) { + instanceAttrs->setTranslationOrigin(getTranslationOriginVal(jCell)); + } // set specified shader type value. May be Unspecified, which means the // default value specified in the stage or object attributes will be used. diff --git a/src/esp/metadata/managers/SceneInstanceAttributesManager.h b/src/esp/metadata/managers/SceneInstanceAttributesManager.h index c7e4e2bc7a..1cf0776b1d 100644 --- a/src/esp/metadata/managers/SceneInstanceAttributesManager.h +++ b/src/esp/metadata/managers/SceneInstanceAttributesManager.h @@ -24,7 +24,7 @@ class SceneInstanceAttributesManager AbstractAttributesManager("Scene Instance", "scene_instance.json") { // build this manager's copy constructor map this->copyConstructorMap_["SceneInstanceAttributes"] = - &SceneInstanceAttributesManager::createObjectCopy< + &SceneInstanceAttributesManager::createObjCopyCtorMapEntry< attributes::SceneInstanceAttributes>; } diff --git a/src/esp/metadata/managers/SemanticAttributesManager.h b/src/esp/metadata/managers/SemanticAttributesManager.h index e6c10150fa..76d3e9adf1 100644 --- a/src/esp/metadata/managers/SemanticAttributesManager.h +++ b/src/esp/metadata/managers/SemanticAttributesManager.h @@ -27,7 +27,7 @@ class SemanticAttributesManager AbstractAttributesManager("Semantic Attributes", "semantic_config.json") { this->copyConstructorMap_["SemanticAttributes"] = - &SemanticAttributesManager::createObjectCopy< + &SemanticAttributesManager::createObjCopyCtorMapEntry< attributes::SemanticAttributes>; } // ctor diff --git a/src/esp/metadata/managers/StageAttributesManager.cpp b/src/esp/metadata/managers/StageAttributesManager.cpp index 9c786856a4..5b611ffa2a 100644 --- a/src/esp/metadata/managers/StageAttributesManager.cpp +++ b/src/esp/metadata/managers/StageAttributesManager.cpp @@ -32,7 +32,8 @@ StageAttributesManager::StageAttributesManager( cfgLightSetup_(NO_LIGHT_KEY) { // build this manager's copy constructor map this->copyConstructorMap_["StageAttributes"] = - &StageAttributesManager::createObjectCopy; + &StageAttributesManager::createObjCopyCtorMapEntry< + attributes::StageAttributes>; } // StageAttributesManager::ctor @@ -42,7 +43,7 @@ void StageAttributesManager::createDefaultPrimBasedAttributesTemplates() { auto tmplt = this->postCreateRegister( StageAttributesManager::initNewObjectInternal("NONE", false), true); std::string tmpltHandle = tmplt->getHandle(); - this->undeletableObjectNames_.insert(std::move(tmpltHandle)); + this->addUndeletableObjectName(std::move(tmpltHandle)); } // StageAttributesManager::createDefaultPrimBasedAttributesTemplates StageAttributes::ptr StageAttributesManager::createPrimBasedAttributesTemplate( @@ -460,8 +461,8 @@ StageAttributesManager::preRegisterObjectFinalize( // accessors are hidden fields this->finalizeAttrPathsBeforeRegister(stageAttributes); - - stageAttributes->setIsClean(); + // Clear dirty flag from when asset handles are changed + stageAttributes->setFilePathsAreClean(); return core::managedContainers::ManagedObjectPreregistration::Success; diff --git a/src/esp/physics/ArticulatedObject.h b/src/esp/physics/ArticulatedObject.h index 6ac578a1b0..54f73bc600 100644 --- a/src/esp/physics/ArticulatedObject.h +++ b/src/esp/physics/ArticulatedObject.h @@ -327,11 +327,11 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { /** * @brief Get a const reference to an ArticulatedLink SceneNode for * info query purposes. - * @param linkId The ArticulatedLink ID or -1 for the baseLink. + * @param linkId The ArticulatedLink ID or @ref BASELINK_ID for the @ref baseLink_. * @return Const reference to the SceneNode. */ - const scene::SceneNode& getLinkSceneNode(int linkId = -1) const { - if (linkId == ID_UNDEFINED) { + const scene::SceneNode& getLinkSceneNode(int linkId = BASELINK_ID) const { + if (linkId == BASELINK_ID) { // base link return baseLink_->node(); } @@ -345,12 +345,12 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { /** * @brief Get pointers to a link's visual SceneNodes. - * @param linkId The ArticulatedLink ID or -1 for the baseLink. + * @param linkId The ArticulatedLink ID or @ref BASELINK_ID for the @ref baseLink_. * @return vector of pointers to the link's visual scene nodes. */ std::vector getLinkVisualSceneNodes( - int linkId = -1) const { - if (linkId == ID_UNDEFINED) { + int linkId = BASELINK_ID) const { + if (linkId == BASELINK_ID) { // base link return baseLink_->visualNodes_; } @@ -404,12 +404,12 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { /** * @brief Get a link by index. * - * @param id The id of the desired link. -1 for base link. + * @param id The id of the desired link. @ref BASELINK_ID for the @ref baseLink_. * @return The desired link. */ ArticulatedLink& getLink(int id) { - // option to get the baseLink_ with id=-1 - if (id == -1) { + // option to get the baseLink_ with id=BASELINK_ID + if (id == BASELINK_ID) { return *baseLink_.get(); } @@ -420,14 +420,16 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { } /** - * @brief Get the number of links for this object (not including the base). + * @brief Get the number of links for this object (not including the @ref baseLink_ + * == @ref BASELINK_ID.). * * @return The number of non-base links. */ int getNumLinks() const { return links_.size(); } /** - * @brief Get a list of link ids, not including the base (-1). + * @brief Get a list of link ids, not including the @ref baseLink_ + * == @ref BASELINK_ID. * * @return A list of link ids for this object. */ @@ -441,14 +443,15 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { } /** - * @brief Get a list of link ids including the base (-1). + * @brief Get a list of link ids including the @ref baseLink_ + * == @ref BASELINK_ID. * * @return A list of link ids for this object. */ std::vector getLinkIdsWithBase() const { std::vector ids; ids.reserve(links_.size() + 1); - ids.push_back(-1); + ids.push_back(BASELINK_ID); for (auto it = links_.begin(); it != links_.end(); ++it) { ids.push_back(it->first); } @@ -488,14 +491,14 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { * @brief Given the list of passed points in this object's local space, return * those points transformed to world space. * @param points vector of points in object local space - * @param linkId Internal link index. + * @param linkId The ArticulatedLink ID or @ref BASELINK_ID for the @ref baseLink_. * @return vector of points transformed into world space */ std::vector transformLocalPointsToWorld( const std::vector& points, - int linkId = -1) const override { - if (linkId == -1) { - return this->baseLink_->transformLocalPointsToWorld(points, -1); + int linkId = BASELINK_ID) const override { + if (linkId == BASELINK_ID) { + return this->baseLink_->transformLocalPointsToWorld(points, BASELINK_ID); } auto linkIter = links_.find(linkId); ESP_CHECK(linkIter != links_.end(), @@ -509,14 +512,14 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { * @brief Given the list of passed points in world space, return * those points transformed to this object's local space. * @param points vector of points in world space - * @param linkId Internal link index. + * @param linkId The ArticulatedLink ID or @ref BASELINK_ID for the @ref baseLink_. * @return vector of points transformed to be in local space */ std::vector transformWorldPointsToLocal( const std::vector& points, - int linkId = -1) const override { - if (linkId == -1) { - return this->baseLink_->transformWorldPointsToLocal(points, -1); + int linkId = BASELINK_ID) const override { + if (linkId == BASELINK_ID) { + return this->baseLink_->transformWorldPointsToLocal(points, BASELINK_ID); } auto linkIter = links_.find(linkId); ESP_CHECK(linkIter != links_.end(), @@ -557,7 +560,7 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { int linkId = getLinkIdFromName(linkName); // locally access the unique pointer's payload const esp::physics::ArticulatedLink* aoLink = nullptr; - if (linkId == -1) { + if (linkId == BASELINK_ID) { aoLink = baseLink_.get(); } else { auto linkIter = links_.find(linkId); @@ -760,11 +763,11 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { /** * @brief Get the name of the link. * - * @param linkId The link's index. -1 for base link. + * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_. * @return The link's name. */ virtual std::string getLinkName(int linkId) const { - if (linkId == -1) { + if (linkId == BASELINK_ID) { return baseLink_->linkName; } @@ -779,15 +782,17 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { * @brief Get the starting position for this link's parent joint in the global * DoFs array. * - * @param linkId The link's index. + * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_. * @return The link's starting DoF index. */ - virtual int getLinkDoFOffset(CORRADE_UNUSED int linkId) const { return -1; } + virtual int getLinkDoFOffset(CORRADE_UNUSED int linkId) const { + return ID_UNDEFINED; + } /** * @brief Get the number of DoFs for this link's parent joint. * - * @param linkId The link's index. + * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_. * @return The number of DoFs for this link's parent joint. */ virtual int getLinkNumDoFs(CORRADE_UNUSED int linkId) const { return 0; } @@ -796,17 +801,17 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { * @brief Get the starting position for this link's parent joint in the global * positions array. * - * @param linkId The link's index. + * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_. * @return The link's starting position index. */ virtual int getLinkJointPosOffset(CORRADE_UNUSED int linkId) const { - return -1; + return ID_UNDEFINED; } /** * @brief Get the number of positions for this link's parent joint. * - * @param linkId The link's index. + * @param linkId The link's index. @ref BASELINK_ID for the @ref baseLink_. * @return The number of positions for this link's parent joint. */ virtual int getLinkNumJointPos(CORRADE_UNUSED int linkId) const { return 0; } @@ -975,7 +980,18 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { return PhysicsObjectBase::getInitObjectInstanceAttrInternal< metadata::attributes::SceneAOInstanceAttributes>(); } - + /** + * @brief Returns a mutable copy of the @ref + * metadata::attributes::SceneAOInstanceAttributes used to place this + * Articulated Object initially in the scene. + * @return a read-only copy of the @ref metadata::attributes::SceneInstanceAttributes used to place + * this object in the scene. + */ + std::shared_ptr + getInitObjectInstanceAttrCopy() const { + return PhysicsObjectBase::getInitObjectInstanceAttrCopyInternal< + metadata::attributes::SceneAOInstanceAttributes>(); + } /** * @brief Return a @ref * metadata::attributes::SceneAOInstanceAttributes reflecting the current diff --git a/src/esp/physics/PhysicsManager.cpp b/src/esp/physics/PhysicsManager.cpp index 712b35041b..35875e2dd5 100644 --- a/src/esp/physics/PhysicsManager.cpp +++ b/src/esp/physics/PhysicsManager.cpp @@ -49,7 +49,6 @@ bool PhysicsManager::initPhysicsFinalize() { //! Create new scene node staticStageObject_ = physics::RigidStage::create(&physicsNode_->createChild(), resourceManager_); - return true; } @@ -57,6 +56,9 @@ PhysicsManager::~PhysicsManager() { ESP_DEBUG() << "Deconstructing PhysicsManager"; } +///////////////////////////////// +// Stage Creation + bool PhysicsManager::addStageInstance( const metadata::attributes::StageAttributes::ptr& initAttributes, const metadata::attributes::SceneObjectInstanceAttributes::cptr& @@ -123,7 +125,6 @@ int PhysicsManager::addObject(int attributesID, int PhysicsManager::addObjectInstance( const esp::metadata::attributes::SceneObjectInstanceAttributes::cptr& objInstAttributes, - bool defaultCOMCorrection, DrawableGroup* drawables, scene::SceneNode* attachmentNode, const std::string& lightSetup) { @@ -194,8 +195,7 @@ int PhysicsManager::addObjectInstance( objInstAttributes->getMassScale()); return addObjectAndSaveAttributes(objAttributes, drawables, attachmentNode, - lightSetup, defaultCOMCorrection, - objInstAttributes); + lightSetup, objInstAttributes); } // PhysicsManager::addObjectInstance @@ -210,16 +210,14 @@ int PhysicsManager::cloneExistingObject(int objectID) { return ID_UNDEFINED; } auto objPtr = existingObjIter->second; - // Get object instance attributes copy - esp::metadata::attributes::SceneObjectInstanceAttributes::cptr objInstAttrs = - objPtr->getInitObjectInstanceAttr(); - // Create object instance - int newObjID = addObjectInstance(objInstAttrs, objPtr->isCOMCorrected(), - &simulator_->getDrawableGroup(), nullptr, - simulator_->getCurrentLightSetupKey()); + // Get object instance attributes copy with current state of object instance + esp::metadata::attributes::SceneObjectInstanceAttributes::ptr + newObjInstAttrs = objPtr->getCurrentStateInstanceAttr(); - // Update new object's values if necessary - // auto newObject = existingObjects_.find(newObjID); + // Create object instance + int newObjID = + addObjectInstance(newObjInstAttrs, &simulator_->getDrawableGroup(), + nullptr, simulator_->getCurrentLightSetupKey()); return newObjID; @@ -230,16 +228,20 @@ int PhysicsManager::addObjectAndSaveAttributes( DrawableGroup* drawables, scene::SceneNode* attachmentNode, const std::string& lightSetup, - bool defaultCOMCorrection, esp::metadata::attributes::SceneObjectInstanceAttributes::cptr objInstAttributes) { - // If no drawables were passed, and a simulator exists - // retrieve a drawable group to use - if ((drawables == nullptr) && (simulator_ != nullptr)) { - // acquire context if available - simulator_->getRenderGLContext(); - // acquire an appropriate drawable group - drawables = &simulator_->getDrawableGroup(); + bool defaultCOMCorrection = false; + if (simulator_ != nullptr) { + // get defaultCOMCorrection from simulator + defaultCOMCorrection = simulator_->getCurSceneDefaultCOMHandling(); + // If no drawables were passed, and a simulator exists + // retrieve a drawable group to use + if (drawables == nullptr) { + // acquire context if available + simulator_->getRenderGLContext(); + // acquire an appropriate drawable group + drawables = &simulator_->getDrawableGroup(); + } } if (objInstAttributes == nullptr) { @@ -387,6 +389,7 @@ int PhysicsManager::addObjectInternal( ///////////////////////////////// // Articulated Object Creation + int PhysicsManager::addArticulatedObject(const std::string& attributesHandle, DrawableGroup* drawables, bool forceReload, @@ -580,17 +583,15 @@ int PhysicsManager::cloneExistingArticulatedObject(int aObjectID) { return ID_UNDEFINED; } auto aObjPtr = existingAOIter->second; - // Get object instance attributes copy - esp::metadata::attributes::SceneAOInstanceAttributes::cptr artObjInstAttrs = - aObjPtr->getInitObjectInstanceAttr(); + // Get articulated object instance attributes copy with current state of AO + esp::metadata::attributes::SceneAOInstanceAttributes::ptr artObjInstAttrs = + aObjPtr->getCurrentStateInstanceAttr(); + // Create object instance int newArtObjID = addArticulatedObjectInstance( artObjInstAttrs, &simulator_->getDrawableGroup(), simulator_->getCurrentLightSetupKey()); - // Update new object's values if necessary - // auto newArtObj = existingArticulatedObjects_.find(newArtObjID); - return newArtObjID; } // PhysicsManager::cloneExistingArticulatedObject @@ -804,7 +805,7 @@ void PhysicsManager::removeObject(const int objectId, trajVisIDByName.erase(trajVisAssetName); // TODO : if object is trajectory visualization, remove its assets as // well once this is supported. - // resourceManager_->removeResourceByName(trajVisAssetName); + // resourceManager_.removeResourceByName(trajVisAssetName); } } // PhysicsManager::removeObject diff --git a/src/esp/physics/PhysicsManager.h b/src/esp/physics/PhysicsManager.h index 29296c449d..65ffbb62d2 100644 --- a/src/esp/physics/PhysicsManager.h +++ b/src/esp/physics/PhysicsManager.h @@ -92,10 +92,13 @@ struct RaycastResults { * @brief based on Bullet b3ContactPointData */ struct ContactPointData { - int objectIdA = -2; // stage is -1 - int objectIdB = -2; - int linkIndexA = -1; // -1 if not a multibody - int linkIndexB = -1; + // Initialize to safe, appropriate values + // stage will be lowest object ID in system + int objectIdA = RIGID_STAGE_ID - 1; + int objectIdB = RIGID_STAGE_ID - 1; + // assume not a multibody + int linkIndexA = ID_UNDEFINED; + int linkIndexB = ID_UNDEFINED; Magnum::Vector3 positionOnAInWS; // contact point location on object A, in // world space coordinates @@ -160,12 +163,11 @@ struct RigidConstraintSettings { /** @brief objectIdB == ID_UNDEFINED indicates "world". */ int objectIdB = ID_UNDEFINED; - /** @brief link of objectA if articulated. ID_UNDEFINED(-1) refers to base. - */ - int linkIdA = ID_UNDEFINED; + /** @brief link of objectA if articulated. @ref BASELINK_ID refers to base.*/ + int linkIdA = BASELINK_ID; - /** @brief link of objectB if articulated. ID_UNDEFINED(-1) refers to base.*/ - int linkIdB = ID_UNDEFINED; + /** @brief link of objectB if articulated. @ref BASELINK_ID refers to base.*/ + int linkIdB = BASELINK_ID; /** @brief constraint point in local space of respective objects*/ Mn::Vector3 pivotA{}, pivotB{}; @@ -264,16 +266,24 @@ class PhysicsManager : public std::enable_shared_from_this { /** * @brief Reset the simulation and physical world. - * Sets the @ref worldTime_ to 0.0, changes the physical state of all objects back to their initial states. Only changes motion_type when scene_instance specified a motion type. - */ - virtual void reset() { + * Sets the @ref worldTime_ to 0.0, changes the physical + * state of all objects back to their initial states. + * Only changes motion_type when scene_instance specified a motion type. + * @param calledAfterSceneCreate If this is true, this is being called + * directly after a new scene was created and all the objects were placed + * appropriately, so bypass object placement reset code. + */ + virtual void reset(bool calledAfterSceneCreate) { // reset object states from initial values (e.g. from scene instance) worldTime_ = 0.0; - for (const auto& bro : existingObjects_) { - bro.second->resetStateFromSceneInstanceAttr(); - } - for (const auto& bao : existingArticulatedObjects_) { - bao.second->resetStateFromSceneInstanceAttr(); + if (!calledAfterSceneCreate) { + // No need to re-place objects after scene creation + for (const auto& bro : existingObjects_) { + bro.second->resetStateFromSceneInstanceAttr(); + } + for (const auto& bao : existingArticulatedObjects_) { + bao.second->resetStateFromSceneInstanceAttr(); + } } } @@ -317,7 +327,6 @@ class PhysicsManager : public std::enable_shared_from_this { int addObjectInstance( const esp::metadata::attributes::SceneObjectInstanceAttributes::cptr& objInstAttributes, - bool defaultCOMCorrection = false, DrawableGroup* drawables = nullptr, scene::SceneNode* attachmentNode = nullptr, const std::string& lightSetup = DEFAULT_LIGHTING_KEY); @@ -983,9 +992,6 @@ class PhysicsManager : public std::enable_shared_from_this { * @param attachmentNode If supplied, attach the new physical object to an * existing SceneNode. * @param lightSetup The string name of the desired lighting setup to use. - * @param defaultCOMCorrection The default value of whether COM-based - * translation correction needs to occur. Only non-default from - * addObjectInstance method. * @param objInstAttributes The attributes that describe the desired state to * set this object on creation. If nullptr, create an empty default instance * and populate it properly based on the object config. @@ -997,7 +1003,6 @@ class PhysicsManager : public std::enable_shared_from_this { DrawableGroup* drawables = nullptr, scene::SceneNode* attachmentNode = nullptr, const std::string& lightSetup = DEFAULT_LIGHTING_KEY, - bool defaultCOMCorrection = false, esp::metadata::attributes::SceneObjectInstanceAttributes::cptr objInstAttributes = nullptr); diff --git a/src/esp/physics/PhysicsObjectBase.h b/src/esp/physics/PhysicsObjectBase.h index ade6e34b3c..b9f70b511c 100644 --- a/src/esp/physics/PhysicsObjectBase.h +++ b/src/esp/physics/PhysicsObjectBase.h @@ -97,10 +97,10 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { */ template std::shared_ptr getInitializationAttributes() const { - if (!initializationAttributes_) { + if (!objInitAttributes_) { return nullptr; } - return T::create(*(static_cast(initializationAttributes_.get()))); + return T::create(*(static_cast(objInitAttributes_.get()))); } /** @@ -229,7 +229,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { */ virtual std::vector transformLocalPointsToWorld( const std::vector& points, - CORRADE_UNUSED int linkID = -1) const { + CORRADE_UNUSED int linkID = ID_UNDEFINED) const { std::vector wsPoints; wsPoints.reserve(points.size()); Mn::Vector3 objScale = getScale(); @@ -249,7 +249,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { */ virtual std::vector transformWorldPointsToLocal( const std::vector& points, - CORRADE_UNUSED int linkID = -1) const { + CORRADE_UNUSED int linkID = ID_UNDEFINED) const { std::vector lsPoints; lsPoints.reserve(points.size()); Mn::Vector3 objScale = getScale(); @@ -479,7 +479,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { template void setSceneInstanceAttr(std::shared_ptr instanceAttr) { - _initObjInstanceAttrs = std::move(instanceAttr); + _objInstanceInitAttributes = std::move(instanceAttr); } // setSceneInstanceAttr /** @@ -584,7 +584,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { for (const auto& markersEntry : linkEntry.second) { const std::string markersName = markersEntry.first; perLinkMap[markersName] = - transformLocalPointsToWorld(markersEntry.second, -1); + transformLocalPointsToWorld(markersEntry.second, ID_UNDEFINED); } perTaskMap[linkName] = perLinkMap; } @@ -593,33 +593,68 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { return res; } // getMarkerPointsGlobal - /** @brief Get the scale of the object set during initialization. + /** + * @brief Get the scale of the object set during initialization. * @return The scaling for the object relative to its initially loaded meshes. */ virtual Magnum::Vector3 getScale() const { return _creationScale; } - /** @brief Return whether or not this object is articulated. Override in - * ArticulatedObject */ + /** + * @brief Return whether or not this object is articulated. Override in + * ArticulatedObject + */ bool isArticulated() const { return _isArticulated; } /** @brief Return the local axis-aligned bounding box of the this object.*/ virtual const Mn::Range3D& getAabb() { return node().getCumulativeBB(); } protected: + /** + * @brief Used Internally on object creation. Set whether or not this object + * is articulated. + */ void setIsArticulated(bool isArticulated) { _isArticulated = isArticulated; } - /** @brief Accessed internally. Get an appropriately cast copy of the @ref + /** + * @brief Accessed internally. Get an appropriately cast copy of the @ref * metadata::attributes::SceneObjectInstanceAttributes used to place the * object within the scene. * @return A copy of the initialization template used to create this object * instance or nullptr if no template exists. */ template - std::shared_ptr getInitObjectInstanceAttrInternal() const { - if (!_initObjInstanceAttrs) { + std::shared_ptr getInitObjectInstanceAttrCopyInternal() const { + if (!_objInstanceInitAttributes) { + return nullptr; + } + static_assert( + std::is_base_of::value, + "SceneObjectInstanceAttributes must be base class of desired instance " + "attributes class."); + return T::create( + *(static_cast(_objInstanceInitAttributes.get()))); + } + + /** + * @brief Accessed internally. Get the + * @ref metadata::attributes::SceneObjectInstanceAttributes used to + * create and place the object within the scene, appropriately cast for + * object type. + * @return A copy of the initialization template used to create this object + * instance or nullptr if no template exists. + */ + template + std::shared_ptr getInitObjectInstanceAttrInternal() const { + if (!_objInstanceInitAttributes) { return nullptr; } - return T::create(*(static_cast(_initObjInstanceAttrs.get()))); + static_assert( + std::is_base_of::value, + "SceneObjectInstanceAttributes must be base class of desired instance " + "attributes class."); + return std::static_pointer_cast(_objInstanceInitAttributes); } /** @@ -639,7 +674,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { */ template std::shared_ptr getCurrentObjectInstanceAttrInternal() { - if (!_initObjInstanceAttrs) { + if (!_objInstanceInitAttributes) { return nullptr; } static_assert( @@ -648,15 +683,37 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { "PhysicsObjectBase : Cast of SceneObjectInstanceAttributes must be to " "class that inherits from SceneObjectInstanceAttributes"); - std::shared_ptr initAttrs = std::const_pointer_cast( - T::create(*(static_cast(_initObjInstanceAttrs.get())))); + std::shared_ptr initObjInstAttrsCopy = std::const_pointer_cast( + T::create(*(static_cast(_objInstanceInitAttributes.get())))); // set values - initAttrs->setTranslation(getUncorrectedTranslation()); - initAttrs->setRotation(getRotation()); - initAttrs->setMotionType( - metadata::attributes::getMotionTypeName(objectMotionType_)); + const auto translation = getUncorrectedTranslation(); + if (initObjInstAttrsCopy->getTranslation() != translation) { + initObjInstAttrsCopy->setTranslation(translation); + } + const auto rotation = getRotation(); + if (initObjInstAttrsCopy->getRotation() != rotation) { + initObjInstAttrsCopy->setRotation(rotation); + } + // only change if different + if (initObjInstAttrsCopy->getMotionType() != objectMotionType_) { + initObjInstAttrsCopy->setMotionType( + metadata::attributes::getMotionTypeName(objectMotionType_)); + } + + // temp copy of object's user attributes. Treated as ground truth for user + // attributes. + core::config::Configuration::ptr tmpUserAttrs = + core::config::Configuration::create(*userAttributes_); - return initAttrs; + // now filter this by the creation attributes' copy. NOTE if the creation + // attributes themselves are different than the same-named versions on disk, + // these values may be out of sync. + tmpUserAttrs->filterFromConfig( + objInitAttributes_->getUserConfigurationView()); + // copy these over the existing user defined fields + // in the instance + initObjInstAttrsCopy->setUserConfiguration(tmpUserAttrs); + return initObjInstAttrsCopy; } /** @@ -711,8 +768,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { /** * @brief Saved template attributes when the object was initialized. */ - metadata::attributes::AbstractAttributes::cptr initializationAttributes_ = - nullptr; + metadata::attributes::AbstractAttributes::cptr objInitAttributes_ = nullptr; /** * @brief Set the object's creation scale @@ -727,7 +783,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { * creation. */ std::shared_ptr - _initObjInstanceAttrs = nullptr; + _objInstanceInitAttributes = nullptr; /** * @brief The scale applied to this object on creation diff --git a/src/esp/physics/RigidBase.h b/src/esp/physics/RigidBase.h index c7d172d7da..a8e0a34e10 100644 --- a/src/esp/physics/RigidBase.h +++ b/src/esp/physics/RigidBase.h @@ -287,6 +287,18 @@ class RigidBase : public esp::physics::PhysicsObjectBase { metadata::attributes::SceneObjectInstanceAttributes>(); } + /** + * @brief Returns a mutable copy of the @ref metadata::attributes::SceneObjectInstanceAttributes + * used to place this rigid object in the scene. + * @return a read-only copy of the @ref metadata::attributes::SceneInstanceAttributes used to place + * this object in the scene. + */ + std::shared_ptr + getInitObjectInstanceAttrCopy() const { + return PhysicsObjectBase::getInitObjectInstanceAttrCopyInternal< + metadata::attributes::SceneObjectInstanceAttributes>(); + } + /** * @brief Return a @ref * metadata::attributes::SceneObjectInstanceAttributes reflecting the current diff --git a/src/esp/physics/RigidObject.cpp b/src/esp/physics/RigidObject.cpp index 245e168df6..d0621ed07c 100644 --- a/src/esp/physics/RigidObject.cpp +++ b/src/esp/physics/RigidObject.cpp @@ -15,7 +15,7 @@ RigidObject::RigidObject(scene::SceneNode* rigidBodyNode, bool RigidObject::initialize( metadata::attributes::AbstractObjectAttributes::ptr initAttributes) { - if (initializationAttributes_ != nullptr) { + if (objInitAttributes_ != nullptr) { ESP_ERROR() << "Cannot initialize a RigidObject more than once"; return false; } @@ -26,7 +26,7 @@ bool RigidObject::initialize( // time setUserAttributes(initAttributes->getUserConfiguration()); setMarkerSets(initAttributes->getMarkerSetsConfiguration()); - initializationAttributes_ = std::move(initAttributes); + objInitAttributes_ = std::move(initAttributes); return initialization_LibSpecific(); } // RigidObject::initialize @@ -37,7 +37,7 @@ bool RigidObject::finalizeObject() { // cast initialization attributes metadata::attributes::ObjectAttributes::cptr ObjectAttributes = std::dynamic_pointer_cast( - initializationAttributes_); + objInitAttributes_); if (!ObjectAttributes->getComputeCOMFromShape()) { // will be false if the COM is provided; shift by that COM @@ -91,7 +91,7 @@ void RigidObject::resetStateFromSceneInstanceAttr() { // set object's motion type if different than set value const physics::MotionType attrObjMotionType = - static_cast(sceneInstanceAttr->getMotionType()); + sceneInstanceAttr->getMotionType(); if (attrObjMotionType != physics::MotionType::UNDEFINED) { this->setMotionType(attrObjMotionType); } diff --git a/src/esp/physics/RigidStage.cpp b/src/esp/physics/RigidStage.cpp index 2d11f37a25..1186658dca 100644 --- a/src/esp/physics/RigidStage.cpp +++ b/src/esp/physics/RigidStage.cpp @@ -13,7 +13,7 @@ RigidStage::RigidStage(scene::SceneNode* rigidBodyNode, bool RigidStage::initialize( metadata::attributes::AbstractObjectAttributes::ptr initAttributes) { - if (initializationAttributes_ != nullptr) { + if (objInitAttributes_ != nullptr) { ESP_ERROR() << "Cannot initialize a RigidStage more than once"; return false; } @@ -26,7 +26,7 @@ bool RigidStage::initialize( // time setUserAttributes(initAttributes->getUserConfiguration()); setMarkerSets(initAttributes->getMarkerSetsConfiguration()); - initializationAttributes_ = std::move(initAttributes); + objInitAttributes_ = std::move(initAttributes); return initialization_LibSpecific(); } diff --git a/src/esp/physics/bullet/BulletArticulatedObject.cpp b/src/esp/physics/bullet/BulletArticulatedObject.cpp index 9a6e15d0b2..eef0c6e965 100644 --- a/src/esp/physics/bullet/BulletArticulatedObject.cpp +++ b/src/esp/physics/bullet/BulletArticulatedObject.cpp @@ -172,7 +172,7 @@ void BulletArticulatedObject::initializeFromURDF( // set user config and initialization attributes setUserAttributes(initAttributes->getUserConfiguration()); setMarkerSets(initAttributes->getMarkerSetsConfiguration()); - initializationAttributes_ = initAttributes; + objInitAttributes_ = initAttributes; } void BulletArticulatedObject::constructStaticRigidBaseObject() { diff --git a/src/esp/physics/bullet/BulletPhysicsManager.cpp b/src/esp/physics/bullet/BulletPhysicsManager.cpp index e31f7867d8..ce09ed0b3d 100644 --- a/src/esp/physics/bullet/BulletPhysicsManager.cpp +++ b/src/esp/physics/bullet/BulletPhysicsManager.cpp @@ -162,7 +162,7 @@ int BulletPhysicsManager::addArticulatedObjectInternal( } // allocate ids for links - ArticulatedLink& rootObject = articulatedObject->getLink(-1); + ArticulatedLink& rootObject = articulatedObject->getLink(BASELINK_ID); rootObject.node().setBaseObjectId(articulatedObject->getObjectID()); for (int linkIx = 0; linkIx < articulatedObject->btMultiBody_->getNumLinks(); ++linkIx) { @@ -536,7 +536,7 @@ void BulletPhysicsManager::lookUpObjectIdAndLinkId( CORRADE_INTERNAL_ASSERT(objectId); CORRADE_INTERNAL_ASSERT(linkId); - *linkId = -1; + *linkId = ID_UNDEFINED; // If the lookup fails, default to the stage. TODO: better error-handling. *objectId = RIGID_STAGE_ID; auto rawColObjIdIter = collisionObjToObjIds_->find(colObj); @@ -572,10 +572,10 @@ std::vector BulletPhysicsManager::getContactPoints() const { const btPersistentManifold* manifold = dispatcher->getInternalManifoldPointer()[i]; - int objectIdA = ID_UNDEFINED; - int objectIdB = ID_UNDEFINED; - int linkIndexA = -1; // -1 if not a multibody - int linkIndexB = -1; + int objectIdA = RIGID_STAGE_ID - 1; + int objectIdB = RIGID_STAGE_ID - 1; + int linkIndexA = ID_UNDEFINED; // -1 if not a multibody + int linkIndexB = ID_UNDEFINED; const btCollisionObject* colObj0 = manifold->getBody0(); const btCollisionObject* colObj1 = manifold->getBody1(); diff --git a/src/esp/physics/bullet/BulletRigidObject.cpp b/src/esp/physics/bullet/BulletRigidObject.cpp index b2dd223308..bdfd2a1460 100644 --- a/src/esp/physics/bullet/BulletRigidObject.cpp +++ b/src/esp/physics/bullet/BulletRigidObject.cpp @@ -424,7 +424,7 @@ void BulletRigidObject::constructAndAddRigidBody(MotionType mt) { } std::string BulletRigidObject::getCollisionDebugName() { - return "RigidObject, " + initializationAttributes_->getHandle() + ", id " + + return "RigidObject, " + objInitAttributes_->getHandle() + ", id " + std::to_string(objectId_); } diff --git a/src/esp/physics/bullet/BulletURDFImporter.cpp b/src/esp/physics/bullet/BulletURDFImporter.cpp index b4c23e6758..8b584b41e9 100644 --- a/src/esp/physics/bullet/BulletURDFImporter.cpp +++ b/src/esp/physics/bullet/BulletURDFImporter.cpp @@ -194,8 +194,9 @@ void BulletURDFImporter::getAllIndices( int mbIndex = cache->getMbIndexFromUrdfIndex(urdfLinkIndex); cp.m_mbIndex = mbIndex; cp.m_parentIndex = parentIndex; - int parentMbIndex = - parentIndex >= 0 ? cache->getMbIndexFromUrdfIndex(parentIndex) : -1; + int parentMbIndex = parentIndex >= 0 + ? cache->getMbIndexFromUrdfIndex(parentIndex) + : BASELINK_ID; cp.m_parentMBIndex = parentMbIndex; allIndices.emplace_back(std::move(cp)); @@ -255,9 +256,8 @@ void BulletURDFImporter::initURDFToBulletCache( cache->m_urdfLinkIndices2BulletLinkIndices.resize( numTotalLinksIncludingBase); cache->m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase); - - cache->m_currentMultiBodyLinkIndex = - -1; // multi body base has 'link' index -1 + // multi body base has 'link' index BASELINK_ID + cache->m_currentMultiBodyLinkIndex = BASELINK_ID; bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER) != 0; if (maintainLinkOrder) { @@ -327,7 +327,7 @@ void BulletURDFImporter::convertURDFToBullet( parentTransforms[urdfLinkIndex] = parentTransformInWorldSpace; std::vector allIndices; - getAllIndices(urdfLinkIndex, -1, allIndices); + getAllIndices(urdfLinkIndex, BASELINK_ID, allIndices); std::sort(allIndices.begin(), allIndices.end(), [](const childParentIndex& a, const childParentIndex& b) { return a.m_index < b.m_index; diff --git a/src/esp/physics/bullet/BulletURDFImporter.h b/src/esp/physics/bullet/BulletURDFImporter.h index 7f37eb17f1..1e806f3fb4 100644 --- a/src/esp/physics/bullet/BulletURDFImporter.h +++ b/src/esp/physics/bullet/BulletURDFImporter.h @@ -54,7 +54,7 @@ struct URDFToBulletCached { std::vector m_urdfLinkIndices2BulletLinkIndices; std::vector m_urdfLinkLocalInertialFrames; - int m_currentMultiBodyLinkIndex{-1}; + int m_currentMultiBodyLinkIndex{BASELINK_ID}; class btMultiBody* m_bulletMultiBody{nullptr}; diff --git a/src/esp/physics/objectManagers/ArticulatedObjectManager.cpp b/src/esp/physics/objectManagers/ArticulatedObjectManager.cpp index 8a11e463b9..959d596eab 100644 --- a/src/esp/physics/objectManagers/ArticulatedObjectManager.cpp +++ b/src/esp/physics/objectManagers/ArticulatedObjectManager.cpp @@ -12,7 +12,8 @@ ArticulatedObjectManager::ArticulatedObjectManager() PhysicsObjectBaseManager("ArticulatedObject") { // build this manager's copy constructor map this->copyConstructorMap_["ManagedArticulatedObject"] = - &ArticulatedObjectManager::createObjectCopy; + &ArticulatedObjectManager::createObjCopyCtorMapEntry< + ManagedArticulatedObject>; // build the function pointers to proper wrapper construction methods, keyed // by the wrapper names @@ -21,7 +22,7 @@ ArticulatedObjectManager::ArticulatedObjectManager() ManagedArticulatedObject>; this->copyConstructorMap_["ManagedBulletArticulatedObject"] = - &ArticulatedObjectManager::createObjectCopy< + &ArticulatedObjectManager::createObjCopyCtorMapEntry< ManagedBulletArticulatedObject>; managedObjTypeConstructorMap_["ManagedBulletArticulatedObject"] = &ArticulatedObjectManager::createPhysicsObjectWrapper< diff --git a/src/esp/physics/objectManagers/RigidObjectManager.cpp b/src/esp/physics/objectManagers/RigidObjectManager.cpp index 27fdc0e00f..b106e4fd7e 100644 --- a/src/esp/physics/objectManagers/RigidObjectManager.cpp +++ b/src/esp/physics/objectManagers/RigidObjectManager.cpp @@ -12,7 +12,7 @@ RigidObjectManager::RigidObjectManager() // build this manager's copy constructor map, keyed by the type name of the // wrappers it will manage this->copyConstructorMap_["ManagedRigidObject"] = - &RigidObjectManager::createObjectCopy; + &RigidObjectManager::createObjCopyCtorMapEntry; // build the function pointers to proper wrapper construction methods, keyed // by the wrapper names @@ -20,7 +20,7 @@ RigidObjectManager::RigidObjectManager() &RigidObjectManager::createPhysicsObjectWrapper; this->copyConstructorMap_["ManagedBulletRigidObject"] = - &RigidObjectManager::createObjectCopy; + &RigidObjectManager::createObjCopyCtorMapEntry; managedObjTypeConstructorMap_["ManagedBulletRigidObject"] = &RigidObjectManager::createPhysicsObjectWrapper; } diff --git a/src/esp/physics/objectWrappers/ManagedArticulatedObject.h b/src/esp/physics/objectWrappers/ManagedArticulatedObject.h index c645531b20..608f577df1 100644 --- a/src/esp/physics/objectWrappers/ManagedArticulatedObject.h +++ b/src/esp/physics/objectWrappers/ManagedArticulatedObject.h @@ -48,7 +48,7 @@ class ManagedArticulatedObject return 1.0; } - scene::SceneNode* getLinkSceneNode(int linkId = -1) const { + scene::SceneNode* getLinkSceneNode(int linkId = BASELINK_ID) const { if (auto sp = getObjectReference()) { return &const_cast(sp->getLinkSceneNode(linkId)); } @@ -56,7 +56,7 @@ class ManagedArticulatedObject } std::vector getLinkVisualSceneNodes( - int linkId = -1) const { + int linkId = BASELINK_ID) const { if (auto sp = getObjectReference()) { return sp->getLinkVisualSceneNodes(linkId); } @@ -73,7 +73,7 @@ class ManagedArticulatedObject if (auto sp = getObjectReference()) { return sp->getNumLinks(); } - return -1; + return ID_UNDEFINED; } std::vector getLinkIds() const { @@ -94,7 +94,7 @@ class ManagedArticulatedObject if (auto sp = getObjectReference()) { return sp->getLinkIdFromName(_name); } - return -1; + return ID_UNDEFINED; } std::unordered_map getLinkObjectIds() const { @@ -237,7 +237,7 @@ class ManagedArticulatedObject if (auto sp = getObjectReference()) { return sp->getLinkDoFOffset(linkId); } - return -1; + return ID_UNDEFINED; } int getLinkNumDoFs(int linkId) const { @@ -251,7 +251,7 @@ class ManagedArticulatedObject if (auto sp = getObjectReference()) { return sp->getLinkJointPosOffset(linkId); } - return -1; + return ID_UNDEFINED; } int getLinkNumJointPos(int linkId) const { diff --git a/src/esp/sim/Simulator.cpp b/src/esp/sim/Simulator.cpp index 8f8dd01d5a..62cd613cd1 100644 --- a/src/esp/sim/Simulator.cpp +++ b/src/esp/sim/Simulator.cpp @@ -426,9 +426,9 @@ bool Simulator::createSceneInstance(const std::string& activeSceneName) { success = instanceArticulatedObjectsForSceneAttributes( curSceneInstanceAttributes_); if (success) { - // TODO : reset may eventually have all the scene instantiation code so - // that scenes can be reset - reset(); + // Pass true so that the object/AO placement code is bypassed in + // physicsManager_->reset + reset(true); } } } @@ -616,12 +616,6 @@ bool Simulator::instanceObjectsForSceneAttributes( // node to attach object to scene::SceneNode* attachmentNode = nullptr; - // whether or not to correct for COM shift - only do for blender-sourced - // scene attributes - bool defaultCOMCorrection = - (curSceneInstanceAttributes_->getTranslationOrigin() == - metadata::attributes::SceneInstanceTranslationOrigin::AssetLocal); - // Iterate through instances, create object and implement initial // transformation. for (const auto& objInst : objectInstances) { @@ -635,8 +629,8 @@ bool Simulator::instanceObjectsForSceneAttributes( config_.activeSceneName)); // objID = - physicsManager_->addObjectInstance(objInst, defaultCOMCorrection, - &getDrawableGroup(), attachmentNode, + physicsManager_->addObjectInstance(objInst, &getDrawableGroup(), + attachmentNode, config_.sceneLightSetupKey); } // for each object attributes return true; @@ -670,12 +664,12 @@ bool Simulator::instanceArticulatedObjectsForSceneAttributes( return true; } // Simulator::instanceArticulatedObjectsForSceneAttributes -void Simulator::reset() { +void Simulator::reset(bool calledAfterSceneCreate) { if (physicsManager_ != nullptr) { // Note: resets time to 0 and all existing objects set back to initial // states. Does not add back deleted objects or delete added objects. Does // not break ManagedObject pointers. - physicsManager_->reset(); + physicsManager_->reset(calledAfterSceneCreate); } for (auto& agent : agents_) { @@ -683,7 +677,7 @@ void Simulator::reset() { } getActiveSceneGraph().getRootNode().computeCumulativeBB(); resourceManager_->setLightSetup(gfx::getDefaultLights()); -} // Simulator::reset() +} // Simulator::reset metadata::attributes::SceneInstanceAttributes::ptr Simulator::buildCurrentStateSceneAttributes() const { diff --git a/src/esp/sim/Simulator.h b/src/esp/sim/Simulator.h index db435c1fba..f19bf6e639 100644 --- a/src/esp/sim/Simulator.h +++ b/src/esp/sim/Simulator.h @@ -75,8 +75,11 @@ class Simulator { * Does not invalidate existing ManagedObject wrappers. * Does not add or remove object instances. * Only changes motion_type when scene_instance specified a motion type. + * @param calledAfterSceneCreate Whether this reset is being called after a + * new scene has been created in reconfigure. If so we con't want to + * redundantly re-place the newly-placed object positions. */ - void reset(); + void reset(bool calledAfterSceneCreate = false); void seed(uint32_t newSeed); @@ -474,6 +477,21 @@ class Simulator { return curSceneInstanceAttributes_->getSimplifiedHandle(); } + /** + * @brief Return whether the @ref + * esp::metadata::attributes::SceneInstanceAttributes used to create the scene + * currently being simulated/displayed specifies a default COM handling for + * rigid objects + */ + + bool getCurSceneDefaultCOMHandling() const { + if (curSceneInstanceAttributes_ == nullptr) { + return false; + } + return (curSceneInstanceAttributes_->getTranslationOrigin() == + metadata::attributes::SceneInstanceTranslationOrigin::AssetLocal); + } + /** * @brief Set the gravity in a physical scene. */ diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index d276dee3af..7d7c7ae766 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -74,7 +74,7 @@ if(MagnumPlugins_KtxImageConverter_FOUND) target_link_libraries(GfxBatchRendererTest PRIVATE MagnumPlugins::KtxImageConverter) endif() -corrade_add_test(CoreTest CoreTest.cpp LIBRARIES core io) +corrade_add_test(ConfigurationTest ConfigurationTest.cpp LIBRARIES core io) corrade_add_test(CullingTest CullingTest.cpp LIBRARIES gfx) target_include_directories(CullingTest PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/src/tests/ConfigurationTest.cpp b/src/tests/ConfigurationTest.cpp new file mode 100644 index 0000000000..29ca1ca979 --- /dev/null +++ b/src/tests/ConfigurationTest.cpp @@ -0,0 +1,589 @@ +// 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. + +#include +#include +#include +#include "esp/core/Configuration.h" +#include "esp/core/Esp.h" + +using namespace esp::core::config; +namespace Cr = Corrade; + +namespace { + +struct ConfigurationTest : Cr::TestSuite::Tester { + explicit ConfigurationTest(); + + /** + * @brief A Configuration is a hierarchical storage structure. This + * recursively builds a subconfiguration hierarchy within the passed @p config + * using the given values. + * @param countPerDepth Number of subconfigs to build for each depth layer. + * @param curDepth Current recursion depth. + * @param totalDepth Total recursion depth +1 == total subconfig hierarchy + * depth of given @p config counting base layer (add 1 because head + * recursion). + * @param config The Configuration in which to build the subconfig hierarchy. + */ + void buildSubconfigsInConfig(int countPerDepth, + int curDepth, + int totalDepth, + Configuration::ptr& config); + + /** + * @brief A Configuration is a hierarchical storage structure. This + * recursively verifies the passed @p config has the subconfig hierarchy + * structure as described by the given values. + * @param countPerDepth Number of subconfigs that should be preseent in each + * depth layer. + * @param curDepth Current recursion depth. + * @param totalDepth Total recursion depth +1 == total subconfig hierarchy + * depth of given @p config counting base layer (add 1 because head + * recursion). + * @param config The Configuration holding the subconfig hierarchy we are + * trying to verify. + */ + void verifySubconfigTree(int countPerDepth, + int curDepth, + int totalDepth, + Configuration::cptr& config) const; + + /** + * @brief Recursively add or modify the string value of the given key in every + * subconfig of @p config with the given value + */ + void addOrModSubconfigTreeVals(const std::string& key, + const std::string& val, + Configuration::ptr& config); + + /** + * @brief Recursively verify that the given key exists in every subconfig of + * @p config with the appropriately modified version of the given value. + */ + void verifySubconfigTreeVals(const std::string& key, + const std::string& val, + Configuration::cptr& config) const; + + /** + * @brief Verifies that the two passed Configurations hold identical + * subconfig hierarchies for both values and structure. + */ + void compareSubconfigs(Configuration::cptr& src, Configuration::cptr& target); + + /** + * @brief Basic test of Configuration functionality + */ + void TestConfiguration(); + + /** + * @brief Test Configuration comparisons between value types requiring fuzzy + * logic (i.e. doubles). All additional numeric types added in the future that + * require fuzzy comparison should have their fuzzy equality bounds tested + * here. + */ + void TestConfigurationFuzzyVals(); + + /** + * @brief Test Configuration find capability. Find returns a list of + * subconfiguration keys required to find a particular key + */ + void TestSubconfigFind(); + + /** + * @brief Test Configuration find, merge and edit capability. Builds + * subconfig hierarchy, finds a desired subconfig, duplicates and merges it + * with another and verifies results. + */ + void TestSubconfigFindAndMerge(); + + /** + * @brief Test Configuration subconfig filtering. Builds two identical + * subconfigs, modifies one such that it has changed values and also added + * values, then filters it using original copy and verifies that only + * new/different values remains. + */ + void TestSubconfigFilter(); + + esp::logging::LoggingContext loggingContext_; +}; // struct ConfigurationTest + +ConfigurationTest::ConfigurationTest() { + addTests({ + &ConfigurationTest::TestConfiguration, + &ConfigurationTest::TestConfigurationFuzzyVals, + &ConfigurationTest::TestSubconfigFind, + &ConfigurationTest::TestSubconfigFindAndMerge, + &ConfigurationTest::TestSubconfigFilter, + }); +} + +void ConfigurationTest::buildSubconfigsInConfig(int countPerDepth, + int curDepth, + int totalDepth, + Configuration::ptr& config) { + if (curDepth == totalDepth) { + return; + } + std::string breadcrumb = config->get("breadcrumb"); + for (int i = 0; i < countPerDepth; ++i) { + const std::string subconfigKey = Cr::Utility::formatString( + "breadcrumb_{}_depth_{}_subconfig_{}", breadcrumb, curDepth, i); + Configuration::ptr newCfg = + config->editSubconfig(subconfigKey); + // set the values within the new subconfig that describe the subconfig's + // status + newCfg->set("depth", curDepth); + newCfg->set("iter", i); + newCfg->set("breadcrumb", Cr::Utility::formatString("{}{}", breadcrumb, i)); + newCfg->set("key", subconfigKey); + + buildSubconfigsInConfig(countPerDepth, curDepth + 1, totalDepth, newCfg); + } +} // ConfigurationTest::buildSubconfigsInConfig + +void ConfigurationTest::verifySubconfigTree(int countPerDepth, + int curDepth, + int totalDepth, + Configuration::cptr& config) const { + if (curDepth == totalDepth) { + return; + } + std::string breadcrumb = config->get("breadcrumb"); + for (int i = 0; i < countPerDepth; ++i) { + const std::string subconfigKey = Cr::Utility::formatString( + "breadcrumb_{}_depth_{}_subconfig_{}", breadcrumb, curDepth, i); + // Verify subconfig with key exists + CORRADE_VERIFY(config->hasSubconfig(subconfigKey)); + Configuration::cptr newCfg = config->getSubconfigView(subconfigKey); + CORRADE_VERIFY(newCfg); + // Verify subconfig depth value is as expected + CORRADE_COMPARE(newCfg->get("depth"), curDepth); + // Verify iteration and parent iteration + CORRADE_COMPARE(newCfg->get("iter"), i); + CORRADE_COMPARE(newCfg->get("breadcrumb"), + Cr::Utility::formatString("{}{}", breadcrumb, i)); + CORRADE_COMPARE(newCfg->get("key"), subconfigKey); + // Check into tree + verifySubconfigTree(countPerDepth, curDepth + 1, totalDepth, newCfg); + } + CORRADE_COMPARE(config->getNumSubconfigs(), countPerDepth); + +} // ConfigurationTest::verifySubconfigTree + +void ConfigurationTest::compareSubconfigs(Configuration::cptr& src, + Configuration::cptr& target) { + // verify target has at least as many subconfigs that src has + CORRADE_VERIFY(src->getNumSubconfigs() <= target->getNumSubconfigs()); + // verify that target has all the values that src has, and they are equal. + auto srcIterValPair = src->getValuesIterator(); + for (auto& cfgIter = srcIterValPair.first; cfgIter != srcIterValPair.second; + ++cfgIter) { + CORRADE_VERIFY(cfgIter->second == target->get(cfgIter->first)); + } + + // Verify all subconfigs have the same keys - get begin/end iterators for src + // subconfigs + auto srcIterConfigPair = src->getSubconfigIterator(); + for (auto& cfgIter = srcIterConfigPair.first; + cfgIter != srcIterConfigPair.second; ++cfgIter) { + CORRADE_VERIFY(target->hasSubconfig(cfgIter->first)); + Configuration::cptr srcSubConfig = cfgIter->second; + Configuration::cptr tarSubConfig = target->getSubconfigView(cfgIter->first); + compareSubconfigs(srcSubConfig, tarSubConfig); + } + +} // ConfigurationTest::compareSubconfigs + +void ConfigurationTest::addOrModSubconfigTreeVals(const std::string& key, + const std::string& val, + Configuration::ptr& config) { + std::string newVal = val; + if (config->hasKeyToValOfType(key, + esp::core::config::ConfigValType::String)) { + newVal = + Cr::Utility::formatString("{}_{}", config->get(key), val); + } + config->set(key, newVal); + auto cfgIterPair = config->getSubconfigIterator(); + // Get subconfig keys + const auto& subsetKeys = config->getSubconfigKeys(); + for (const auto& subKey : subsetKeys) { + auto subconfig = config->editSubconfig(subKey); + addOrModSubconfigTreeVals(key, val, subconfig); + } +} // ConfigurationTest::addOrModSubconfigTreeVals + +void ConfigurationTest::verifySubconfigTreeVals( + const std::string& key, + const std::string& val, + Configuration::cptr& config) const { + CORRADE_VERIFY( + config->hasKeyToValOfType(key, esp::core::config::ConfigValType::String)); + std::string testVal = config->get(key); + std::size_t foundLoc = testVal.find(val); + CORRADE_VERIFY(foundLoc != std::string::npos); + auto srcIterConfigPair = config->getSubconfigIterator(); + for (auto& cfgIter = srcIterConfigPair.first; + cfgIter != srcIterConfigPair.second; ++cfgIter) { + Configuration::cptr subCfg = cfgIter->second; + verifySubconfigTreeVals(key, val, subCfg); + } +} // ConfigurationTest::verifySubconfigTreeVals + +void ConfigurationTest::TestConfiguration() { + Configuration cfg; + cfg.set("myBool", true); + cfg.set("myInt", 10); + cfg.set("myFloatToDouble", 1.2f); + cfg.set("myVec2", Mn::Vector2{1.0, 2.0}); + cfg.set("myVec3", Mn::Vector3{1.0, 2.0, 3.0}); + cfg.set("myVec4", Mn::Vector4{1.0, 2.0, 3.0, 4.0}); + cfg.set("myQuat", Mn::Quaternion{{1.0, 2.0, 3.0}, 0.1}); + cfg.set("myMat3", Mn::Matrix3(Mn::Math::IdentityInit)); + cfg.set("myMat4", Mn::Matrix4(Mn::Math::IdentityInit)); + cfg.set("myRad", Mn::Rad{1.23}); + cfg.set("myString", "test"); + + CORRADE_VERIFY(cfg.hasValue("myBool")); + CORRADE_VERIFY(cfg.hasValue("myInt")); + CORRADE_VERIFY(cfg.hasValue("myFloatToDouble")); + CORRADE_VERIFY(cfg.hasValue("myVec2")); + CORRADE_VERIFY(cfg.hasValue("myVec3")); + CORRADE_VERIFY(cfg.hasValue("myMat3")); + CORRADE_VERIFY(cfg.hasValue("myVec4")); + CORRADE_VERIFY(cfg.hasValue("myQuat")); + CORRADE_VERIFY(cfg.hasValue("myMat3")); + CORRADE_VERIFY(cfg.hasValue("myRad")); + CORRADE_VERIFY(cfg.hasValue("myString")); + + CORRADE_COMPARE(cfg.get("myBool"), true); + CORRADE_COMPARE(cfg.get("myInt"), 10); + CORRADE_COMPARE(cfg.get("myFloatToDouble"), 1.2f); + CORRADE_COMPARE(cfg.get("myVec2"), Mn::Vector2(1.0, 2.0)); + CORRADE_COMPARE(cfg.get("myVec3"), Mn::Vector3(1.0, 2.0, 3.0)); + CORRADE_COMPARE(cfg.get("myVec4"), + Mn::Vector4(1.0, 2.0, 3.0, 4.0)); + CORRADE_COMPARE(cfg.get("myQuat"), + Mn::Quaternion({1.0, 2.0, 3.0}, 0.1)); + CORRADE_COMPARE(cfg.get("myRad"), Mn::Rad(1.23)); + + for (int i = 0; i < 3; ++i) { + CORRADE_COMPARE(cfg.get("myMat3").row(i)[i], 1); + } + for (int i = 0; i < 4; ++i) { + CORRADE_COMPARE(cfg.get("myMat4").row(i)[i], 1); + } + + CORRADE_COMPARE(cfg.get("myString"), "test"); +} // ConfigurationTest::TestConfiguration test + +void ConfigurationTest::TestConfigurationFuzzyVals() { + Configuration cfg; + + // Specify values to test + cfg.set("fuzzyTestVal0", 1.0); + cfg.set("fuzzyTestVal1", 1.0 + Mn::Math::TypeTraits::epsilon() / 2); + // Scale the epsilon to be too big to be seen as the same. + cfg.set("fuzzyTestVal2", 1.0 + Mn::Math::TypeTraits::epsilon() * 4); + + CORRADE_VERIFY(cfg.hasValue("fuzzyTestVal0")); + CORRADE_VERIFY(cfg.hasValue("fuzzyTestVal1")); + CORRADE_VERIFY(cfg.hasValue("fuzzyTestVal2")); + // Verify very close doubles are considered sufficiently close by fuzzy + // compare + CORRADE_COMPARE(cfg.get("fuzzyTestVal0"), cfg.get("fuzzyTestVal1")); + + // verify very close but not-quite-close enough doubles are considered + // different by magnum's fuzzy compare + CORRADE_COMPARE_AS(cfg.get("fuzzyTestVal0"), cfg.get("fuzzyTestVal2"), + Cr::TestSuite::Compare::NotEqual); + +} // ConfigurationTest::TestConfigurationFuzzyVals + +// Test configuration find functionality +void ConfigurationTest::TestSubconfigFind() { + Configuration::ptr cfg = Configuration::create(); + Configuration::ptr baseCfg = cfg; + // Build layers of subconfigs + for (int i = 0; i < 10; ++i) { + Configuration::ptr newCfg = cfg->editSubconfig( + Cr::Utility::formatString("depth_{}_subconfig_{}", i + 1, i)); + newCfg->set("depth", i + 1); + cfg = newCfg; + } + // Set deepest layer config to have treasure + cfg->set("treasure", "this is the treasure!"); + // Find the treasure! + std::vector keyList = baseCfg->findValue("treasure"); + + // Display breadcrumbs + std::string resStr = Cr::Utility::formatString( + "Vector of 'treasure' keys is size : {}", keyList.size()); + for (const auto& key : keyList) { + Cr::Utility::formatInto(resStr, resStr.size(), "\n\t{}", key); + } + ESP_DEBUG() << resStr; + // Verify the found treasure + CORRADE_COMPARE(keyList.size(), 11); + + Configuration::cptr viewConfig = baseCfg; + for (int i = 0; i < 10; ++i) { + const std::string subconfigKey = + Cr::Utility::formatString("depth_{}_subconfig_{}", i + 1, i); + CORRADE_COMPARE(keyList[i], subconfigKey); + // Verify that subconfig with given name exists + CORRADE_VERIFY(viewConfig->hasSubconfig(subconfigKey)); + // retrieve actual subconfig + Configuration::cptr newCfg = viewConfig->getSubconfigView(subconfigKey); + // verity subconfig has correct depth value + CORRADE_COMPARE(newCfg->get("depth"), i + 1); + viewConfig = newCfg; + } + CORRADE_VERIFY(viewConfig->hasValue("treasure")); + CORRADE_COMPARE(viewConfig->get("treasure"), + "this is the treasure!"); + + // Verify pirate isn't found + std::vector notFoundKeyList = baseCfg->findValue("pirate"); + CORRADE_COMPARE(notFoundKeyList.size(), 0); +} + +// Test subconfig edit/merge and save +void ConfigurationTest::TestSubconfigFindAndMerge() { + Configuration::ptr cfg = Configuration::create(); + CORRADE_VERIFY(cfg); + // Set base value for parent iteration + cfg->set("breadcrumb", ""); + int depthPlus1 = 5; + int count = 3; + // Build subconfig tree 4 levels deep, 3 subconfigs per config + buildSubconfigsInConfig(count, 0, depthPlus1, cfg); + Configuration::cptr const_cfg = + std::const_pointer_cast(cfg); + // Verify subconfig tree structure using const views + verifySubconfigTree(count, 0, depthPlus1, const_cfg); + + // grab a subconfig, edit it and save it with a different key + // use find to get key path + const std::string keyToFind = "breadcrumb_212_depth_3_subconfig_2"; + // Find breadcrumb path of keys into subconfigs that lead to keyToFind + std::vector subconfigKeyPath = cfg->findValue(keyToFind); + // Display breadcrumbs + std::string resStr = Cr::Utility::formatString( + "Vector of desired subconfig keys to get to '{}' " + "subconfig is size : {}", + keyToFind, subconfigKeyPath.size()); + for (const auto& key : subconfigKeyPath) { + Cr::Utility::formatInto(resStr, resStr.size(), "\n\t{}", key); + } + ESP_DEBUG() << resStr; + // Verify there are 4 layers of subconfigs to keyToFind + CORRADE_COMPARE(subconfigKeyPath.size(), 4); + // Verify the last entry holds keyToFind + CORRADE_COMPARE(subconfigKeyPath[3], keyToFind); + // Procede through the subconfig layers and find the desired target config + Configuration::cptr viewConfig = cfg; + Configuration::cptr lastConfig = cfg; + for (const std::string& key : subconfigKeyPath) { + Configuration::cptr tempConfig = viewConfig->getSubconfigView(key); + CORRADE_COMPARE(tempConfig->get("key"), key); + lastConfig = viewConfig; + viewConfig = tempConfig; + } + + // By here lastConfig is the parent of viewConfig, the config we want at + // keyToFind + // We make a copy of viewConfig and verify the copy has the same values as the + // original + Configuration::ptr cfgToEdit = + lastConfig->getSubconfigCopy(keyToFind); + CORRADE_VERIFY(*cfgToEdit == *viewConfig); + // Now modify new subconfig copy and verify the original viewConfig is not + // also modified + const std::string newKey = "edited_subconfig"; + cfgToEdit->set("key", newKey); + cfgToEdit->set("depth", 0); + cfgToEdit->set("breadcrumb", ""); + cfgToEdit->set("test_string", "this is an added test string"); + + // Added edited subconfig to base config (top of hierarchy) + cfg->setSubconfigPtr(newKey, cfgToEdit); + CORRADE_VERIFY(cfg->hasSubconfig(newKey)); + // Get an immutable view of this subconfig and verify the data it holds + Configuration::cptr cfgToVerify = cfg->getSubconfigView(newKey); + CORRADE_COMPARE(cfgToVerify->get("breadcrumb"), ""); + CORRADE_COMPARE(cfgToVerify->get("test_string"), + "this is an added test string"); + CORRADE_COMPARE(cfgToVerify->get("key"), newKey); + CORRADE_COMPARE(cfgToVerify->get("depth"), 0); + // Make sure original was not modified + CORRADE_VERIFY(viewConfig->get("breadcrumb") != ""); + CORRADE_VERIFY(viewConfig->get("key") != newKey); + CORRADE_VERIFY(viewConfig->get("depth") != 0); + CORRADE_VERIFY(!viewConfig->hasValue("test_string")); + + // Now Test merge/Overwriting + const std::string mergedKey = "merged_subconfig"; + Configuration::ptr cfgToOverwrite = + cfg->editSubconfig(mergedKey); + + // first set some test values that will be clobbered + cfgToOverwrite->set("key", mergedKey); + cfgToOverwrite->set("depth", 11); + cfgToOverwrite->set("breadcrumb", "1123"); + cfgToOverwrite->set("test_string", "this string will be clobbered"); + // Now add some values that won't be clobbered + cfgToOverwrite->set("myBool", true); + cfgToOverwrite->set("myInt", 10); + cfgToOverwrite->set("myFloatToDouble", 1.2f); + cfgToOverwrite->set("myVec2", Mn::Vector2{1.0, 2.0}); + cfgToOverwrite->set("myVec3", Mn::Vector3{1.0, 2.0, 3.0}); + cfgToOverwrite->set("myVec4", Mn::Vector4{1.0, 2.0, 3.0, 4.0}); + cfgToOverwrite->set("myQuat", Mn::Quaternion{{1.0, 2.0, 3.0}, 0.1}); + cfgToOverwrite->set("myRad", Mn::Rad{1.23}); + cfgToOverwrite->set("myString", "test"); + + // Now overwrite the values from cfgToVerify + cfgToOverwrite->overwriteWithConfig(cfgToVerify); + + CORRADE_VERIFY(cfg->hasSubconfig(mergedKey)); + // Verify all the overwritten values are correct + Configuration::cptr cfgToVerifyOverwrite = cfg->getSubconfigView(mergedKey); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("breadcrumb"), ""); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("test_string"), + "this is an added test string"); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("key"), newKey); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("depth"), 0); + // Verify original non-overwritten values are still present + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myBool"), true); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myInt"), 10); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myFloatToDouble"), 1.2f); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myVec2"), + Mn::Vector2(1.0, 2.0)); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myVec3"), + Mn::Vector3(1.0, 2.0, 3.0)); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myVec4"), + Mn::Vector4(1.0, 2.0, 3.0, 4.0)); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myQuat"), + Mn::Quaternion({1.0, 2.0, 3.0}, 0.1)); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myRad"), Mn::Rad(1.23)); + CORRADE_COMPARE(cfgToVerifyOverwrite->get("myString"), "test"); + // Verify overwrite performed properly + compareSubconfigs(cfgToVerify, cfgToVerifyOverwrite); +} + +void ConfigurationTest::TestSubconfigFilter() { + Configuration::ptr cfg = Configuration::create(); + CORRADE_VERIFY(cfg); + // Build and verify hierarchy + int depthPlus1 = 5; + int count = 3; + // build two identical subconfigs and place within base Configuration + const std::string filterKey = "cfgToFilterBy"; + // Get actual subconfig + Configuration::ptr cfgToFilterBy = + cfg->editSubconfig(filterKey); + // add some root values will be present in 'modified' subconfig too + cfgToFilterBy->set("baseStrKey", "filterKey"); + cfgToFilterBy->set("baseIntKey", 1); + cfgToFilterBy->set("baseBoolKey", true); + cfgToFilterBy->set("baseDoubleKey", 3.14); + cfgToFilterBy->set("baseStrKeyToMod", "modFilterKey"); + cfgToFilterBy->set("baseIntKeyToMod", 11); + cfgToFilterBy->set("baseBoolKeyToMod", true); + cfgToFilterBy->set("baseDoubleKeyToMod", 2.718); + // Build subconfig tree 4 levels deep, 3 subconfigs per config + buildSubconfigsInConfig(count, 0, depthPlus1, cfgToFilterBy); + + const std::string modKey = "modifiedSubconfig"; + // get copy of original filter config + Configuration::ptr cfgToModify = + cfg->getSubconfigCopy(filterKey); + // move copy into cfg + cfg->setSubconfigPtr(modKey, cfgToModify); + // retrieve pointer to new copy + cfgToModify = cfg->editSubconfig(modKey); + + // Build subconfig tree 4 levels deep, 3 subconfigs per config + buildSubconfigsInConfig(count, 0, depthPlus1, cfgToModify); + + // compare both configs and verify they are equal + { + Configuration::cptr baseCompareCfg = + std::static_pointer_cast(cfgToFilterBy); + Configuration::cptr modCompareCfg = + std::static_pointer_cast(cfgToModify); + // verify both subconfigs are the same + compareSubconfigs(baseCompareCfg, modCompareCfg); + } + + // modify and save appropriate subconfig, filter using base subconfig, and + // then verify expected results + // Modify values that are also present in cfgToFilterBy + cfgToModify->set("baseStrKeyToMod", "_modified"); + cfgToModify->set("baseIntKeyToMod", 111); + cfgToModify->set("baseBoolKeyToMod", false); + cfgToModify->set("baseDoubleKeyToMod", 1234.5); + // Add 'mod' string to end of each config's breadcrumb string + addOrModSubconfigTreeVals("breadcrumb", "mod", cfgToModify); + // Add new string to each config + addOrModSubconfigTreeVals("newStrValue", "new string", cfgToModify); + // Add values that are not present in cfgToFilterBy + cfgToModify->set("myBool", true); + cfgToModify->set("myInt", 10); + cfgToModify->set("myFloatToDouble", 1.2f); + cfgToModify->set("myVec2", Mn::Vector2{1.0, 2.0}); + cfgToModify->set("myVec3", Mn::Vector3{1.0, 2.0, 3.0}); + cfgToModify->set("myVec4", Mn::Vector4{1.0, 2.0, 3.0, 4.0}); + cfgToModify->set("myQuat", Mn::Quaternion{{1.0, 2.0, 3.0}, 0.1}); + cfgToModify->set("myRad", Mn::Rad{1.23}); + cfgToModify->set("myString", "test"); + + // Now filter the config to not have any data shared ith cfgToFilterBy + cfgToModify->filterFromConfig(cfgToFilterBy); + // Verify old shared values are gone + CORRADE_VERIFY(!cfgToModify->hasValue("baseStrKey")); + CORRADE_VERIFY(!cfgToModify->hasValue("baseIntKey")); + CORRADE_VERIFY(!cfgToModify->hasValue("baseBoolKey")); + CORRADE_VERIFY(!cfgToModify->hasValue("baseDoubleKey")); + + // Verify modified shared values are present + CORRADE_VERIFY(cfgToModify->hasValue("baseStrKeyToMod")); + CORRADE_VERIFY(cfgToModify->hasValue("baseIntKeyToMod")); + CORRADE_VERIFY(cfgToModify->hasValue("baseBoolKeyToMod")); + CORRADE_VERIFY(cfgToModify->hasValue("baseDoubleKeyToMod")); + // ...and have expected values + CORRADE_COMPARE(cfgToModify->get("baseStrKeyToMod"), + "_modified"); + CORRADE_COMPARE(cfgToModify->get("baseIntKeyToMod"), 111); + CORRADE_COMPARE(cfgToModify->get("baseBoolKeyToMod"), false); + CORRADE_COMPARE(cfgToModify->get("baseDoubleKeyToMod"), 1234.5); + + // Verify modified values still present + CORRADE_COMPARE(cfgToModify->get("myBool"), true); + CORRADE_COMPARE(cfgToModify->get("myInt"), 10); + CORRADE_COMPARE(cfgToModify->get("myFloatToDouble"), 1.2f); + CORRADE_COMPARE(cfgToModify->get("myVec2"), + Mn::Vector2(1.0, 2.0)); + CORRADE_COMPARE(cfgToModify->get("myVec3"), + Mn::Vector3(1.0, 2.0, 3.0)); + CORRADE_COMPARE(cfgToModify->get("myVec4"), + Mn::Vector4(1.0, 2.0, 3.0, 4.0)); + CORRADE_COMPARE(cfgToModify->get("myQuat"), + Mn::Quaternion({1.0, 2.0, 3.0}, 0.1)); + CORRADE_COMPARE(cfgToModify->get("myRad"), Mn::Rad(1.23)); + CORRADE_COMPARE(cfgToModify->get("myString"), "test"); + + Configuration::cptr constModCfg = + std::const_pointer_cast(cfgToModify); + // verify breadcrumb mod is present + verifySubconfigTreeVals("breadcrumb", "mod", constModCfg); + // verify newStrValue is present + verifySubconfigTreeVals("newStrValue", "new string", constModCfg); +} // ConfigurationTest::TestSubconfigFilter + +} // namespace + +CORRADE_TEST_MAIN(ConfigurationTest) diff --git a/src/tests/CoreTest.cpp b/src/tests/CoreTest.cpp deleted file mode 100644 index 2bef3e8a99..0000000000 --- a/src/tests/CoreTest.cpp +++ /dev/null @@ -1,334 +0,0 @@ -// 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. - -#include -#include -#include "esp/core/Configuration.h" -#include "esp/core/Esp.h" - -using namespace esp::core::config; -namespace Cr = Corrade; - -namespace { - -struct CoreTest : Cr::TestSuite::Tester { - explicit CoreTest(); - - void buildSubconfigsInConfig(int countPerDepth, - int curDepth, - int totalDepth, - Configuration::ptr& config); - - void verifySubconfigTree(int countPerDepth, - int curDepth, - int totalDepth, - Configuration::cptr& config); - - void compareSubconfigs(Configuration::cptr& src, Configuration::cptr& target); - - void TestConfiguration(); - - /** - * @brief Test Configuration find and subconfig edit capability. Find returns - * a list of subconfiguration keys required to find a particular key - */ - void TestConfigurationSubconfigFind(); - - esp::logging::LoggingContext loggingContext_; -}; // struct CoreTest - -CoreTest::CoreTest() { - addTests({ - &CoreTest::TestConfiguration, - &CoreTest::TestConfigurationSubconfigFind, - }); -} - -void CoreTest::buildSubconfigsInConfig(int countPerDepth, - int curDepth, - int totalDepth, - Configuration::ptr& config) { - if (curDepth == totalDepth) { - return; - } - std::string breadcrumb = config->get("breakcrumb"); - for (int i = 0; i < countPerDepth; ++i) { - const std::string subconfigKey = Cr::Utility::formatString( - "breadcrumb_{}_depth_{}_subconfig_{}", breadcrumb, curDepth, i); - Configuration::ptr newCfg = - config->editSubconfig(subconfigKey); - newCfg->set("depth", curDepth); - newCfg->set("iter", i); - newCfg->set("breakcrumb", Cr::Utility::formatString("{}{}", breadcrumb, i)); - newCfg->set("key", subconfigKey); - - buildSubconfigsInConfig(countPerDepth, curDepth + 1, totalDepth, newCfg); - } -} // CoreTest::buildSubconfigsInConfig - -void CoreTest::verifySubconfigTree(int countPerDepth, - int curDepth, - int totalDepth, - Configuration::cptr& config) { - if (curDepth == totalDepth) { - return; - } - std::string breadcrumb = config->get("breakcrumb"); - for (int i = 0; i < countPerDepth; ++i) { - const std::string subconfigKey = Cr::Utility::formatString( - "breadcrumb_{}_depth_{}_subconfig_{}", breadcrumb, curDepth, i); - // Verify subconfig with key exists - CORRADE_VERIFY(config->hasSubconfig(subconfigKey)); - Configuration::cptr newCfg = config->getSubconfigView(subconfigKey); - CORRADE_VERIFY(newCfg); - // Verify subconfig depth value is as expected - CORRADE_COMPARE(newCfg->get("depth"), curDepth); - // Verify iteration and parent iteration - CORRADE_COMPARE(newCfg->get("iter"), i); - CORRADE_COMPARE(newCfg->get("breakcrumb"), - Cr::Utility::formatString("{}{}", breadcrumb, i)); - CORRADE_COMPARE(newCfg->get("key"), subconfigKey); - // Check into tree - verifySubconfigTree(countPerDepth, curDepth + 1, totalDepth, newCfg); - } - CORRADE_COMPARE(config->getNumSubconfigs(), countPerDepth); - -} // CoreTest::verifySubconfigTree - -void CoreTest::compareSubconfigs(Configuration::cptr& src, - Configuration::cptr& target) { - // verify target has at least all the number of subconfigs that src has - CORRADE_VERIFY(src->getNumSubconfigs() <= target->getNumSubconfigs()); - // verify that target has all the values that src has, and they are equal. - auto srcIterValPair = src->getValuesIterator(); - for (auto& cfgIter = srcIterValPair.first; cfgIter != srcIterValPair.second; - ++cfgIter) { - CORRADE_VERIFY(cfgIter->second == target->get(cfgIter->first)); - } - - // Verify all subconfigs have the same keys - get begin/end iterators for src - // subconfigs - auto srcIterConfigPair = src->getSubconfigIterator(); - for (auto& cfgIter = srcIterConfigPair.first; - cfgIter != srcIterConfigPair.second; ++cfgIter) { - CORRADE_VERIFY(target->hasSubconfig(cfgIter->first)); - Configuration::cptr srcSubConfig = cfgIter->second; - Configuration::cptr tarSubConfig = target->getSubconfigView(cfgIter->first); - compareSubconfigs(srcSubConfig, tarSubConfig); - } - -} // CoreTest::compareSubconfigs - -void CoreTest::TestConfiguration() { - Configuration cfg; - cfg.set("myBool", true); - cfg.set("myInt", 10); - cfg.set("myFloatToDouble", 1.2f); - cfg.set("myVec2", Mn::Vector2{1.0, 2.0}); - cfg.set("myVec3", Mn::Vector3{1.0, 2.0, 3.0}); - cfg.set("myVec4", Mn::Vector4{1.0, 2.0, 3.0, 4.0}); - cfg.set("myQuat", Mn::Quaternion{{1.0, 2.0, 3.0}, 0.1}); - cfg.set("myMat3", Mn::Matrix3(Mn::Math::IdentityInit)); - cfg.set("myMat4", Mn::Matrix4(Mn::Math::IdentityInit)); - cfg.set("myRad", Mn::Rad{1.23}); - cfg.set("myString", "test"); - - CORRADE_VERIFY(cfg.hasValue("myBool")); - CORRADE_VERIFY(cfg.hasValue("myInt")); - CORRADE_VERIFY(cfg.hasValue("myFloatToDouble")); - CORRADE_VERIFY(cfg.hasValue("myVec2")); - CORRADE_VERIFY(cfg.hasValue("myVec3")); - CORRADE_VERIFY(cfg.hasValue("myMat3")); - CORRADE_VERIFY(cfg.hasValue("myVec4")); - CORRADE_VERIFY(cfg.hasValue("myQuat")); - CORRADE_VERIFY(cfg.hasValue("myMat3")); - CORRADE_VERIFY(cfg.hasValue("myRad")); - CORRADE_VERIFY(cfg.hasValue("myString")); - - CORRADE_COMPARE(cfg.get("myBool"), true); - CORRADE_COMPARE(cfg.get("myInt"), 10); - CORRADE_COMPARE(cfg.get("myFloatToDouble"), 1.2f); - CORRADE_COMPARE(cfg.get("myVec2"), Mn::Vector2(1.0, 2.0)); - CORRADE_COMPARE(cfg.get("myVec3"), Mn::Vector3(1.0, 2.0, 3.0)); - CORRADE_COMPARE(cfg.get("myVec4"), - Mn::Vector4(1.0, 2.0, 3.0, 4.0)); - CORRADE_COMPARE(cfg.get("myQuat"), - Mn::Quaternion({1.0, 2.0, 3.0}, 0.1)); - CORRADE_COMPARE(cfg.get("myRad"), Mn::Rad(1.23)); - - for (int i = 0; i < 3; ++i) { - CORRADE_COMPARE(cfg.get("myMat3").row(i)[i], 1); - } - for (int i = 0; i < 4; ++i) { - CORRADE_COMPARE(cfg.get("myMat4").row(i)[i], 1); - } - - CORRADE_COMPARE(cfg.get("myString"), "test"); -} // CoreTest::TestConfiguration test - -// Test configuration find functionality -void CoreTest::TestConfigurationSubconfigFind() { - { - Configuration::ptr cfg = Configuration::create(); - Configuration::ptr baseCfg = cfg; - // Build layers of subconfigs - for (int i = 0; i < 10; ++i) { - Configuration::ptr newCfg = cfg->editSubconfig( - Cr::Utility::formatString("depth_{}_subconfig_{}", i + 1, i)); - newCfg->set("depth", i + 1); - cfg = newCfg; - } - // Set deepest layer config to have treasure - cfg->set("treasure", "this is the treasure!"); - // Find the treasure! - std::vector keyList = baseCfg->findValue("treasure"); - - // Display breadcrumbs - std::string resStr = Cr::Utility::formatString( - "Vector of 'treasure' keys is size : {}", keyList.size()); - for (const auto& key : keyList) { - Cr::Utility::formatInto(resStr, resStr.size(), "\n\t{}", key); - } - ESP_DEBUG() << resStr; - // Verify the found treasure - CORRADE_COMPARE(keyList.size(), 11); - - Configuration::cptr viewConfig = baseCfg; - for (int i = 0; i < 10; ++i) { - const std::string subconfigKey = - Cr::Utility::formatString("depth_{}_subconfig_{}", i + 1, i); - CORRADE_COMPARE(keyList[i], subconfigKey); - // Verify that subconfig with given name exists - CORRADE_VERIFY(viewConfig->hasSubconfig(subconfigKey)); - // retrieve actual subconfig - Configuration::cptr newCfg = viewConfig->getSubconfigView(subconfigKey); - // verity subconfig has correct depth value - CORRADE_COMPARE(newCfg->get("depth"), i + 1); - viewConfig = newCfg; - } - CORRADE_VERIFY(viewConfig->hasValue("treasure")); - CORRADE_COMPARE(viewConfig->get("treasure"), - "this is the treasure!"); - - // Verify pirate isn't found - std::vector notFoundKeyList = baseCfg->findValue("pirate"); - CORRADE_COMPARE(notFoundKeyList.size(), 0); - } - // Test subconfig edit/merge and save - { - Configuration::ptr cfg = Configuration::create(); - CORRADE_VERIFY(cfg); - // Set base value for parent iteration - cfg->set("breakcrumb", ""); - int depth = 5; - int count = 3; - // Build subconfig tree 4 levels deep, 3 subconfigs per config - buildSubconfigsInConfig(count, 0, depth, cfg); - Configuration::cptr const_cfg = - std::const_pointer_cast(cfg); - // Verify subconfig tree structure using const views - verifySubconfigTree(count, 0, depth, const_cfg); - - // grab a subconfig, edit it and save it with a different key - // use find to get key path - const std::string keyToFind = "breadcrumb_212_depth_3_subconfig_2"; - std::vector subconfigPath = cfg->findValue(keyToFind); - // Display breadcrumbs - std::string resStr = Cr::Utility::formatString( - "Vector of desired subconfig keys to get to '{}' " - "subconfig is size : {}", - keyToFind, subconfigPath.size()); - for (const auto& key : subconfigPath) { - Cr::Utility::formatInto(resStr, resStr.size(), "\n\t{}", key); - } - ESP_DEBUG() << resStr; - CORRADE_COMPARE(subconfigPath.size(), 4); - CORRADE_COMPARE(subconfigPath[3], keyToFind); - Configuration::cptr viewConfig = cfg; - Configuration::cptr lastConfig = cfg; - for (const std::string& key : subconfigPath) { - Configuration::cptr tempConfig = viewConfig->getSubconfigView(key); - CORRADE_COMPARE(tempConfig->get("key"), key); - lastConfig = viewConfig; - viewConfig = tempConfig; - } - // By here lastConfig is the parent of the config we want, and viewConfig is - // the config we are getting a copy of. - Configuration::ptr cfgToEdit = - lastConfig->getSubconfigCopy(keyToFind); - const std::string newKey = "edited_subconfig"; - cfgToEdit->set("key", newKey); - cfgToEdit->set("depth", 0); - cfgToEdit->set("breakcrumb", ""); - cfgToEdit->set("test_string", "this is an added test string"); - // Added edited subconfig to base config - cfg->setSubconfigPtr(newKey, cfgToEdit); - CORRADE_VERIFY(cfg->hasSubconfig(newKey)); - Configuration::cptr cfgToVerify = cfg->getSubconfigView(newKey); - CORRADE_COMPARE(cfgToVerify->get("breakcrumb"), ""); - CORRADE_COMPARE(cfgToVerify->get("test_string"), - "this is an added test string"); - CORRADE_COMPARE(cfgToVerify->get("key"), newKey); - CORRADE_COMPARE(cfgToVerify->get("depth"), 0); - // Make sure original was not modified - CORRADE_VERIFY(viewConfig->get("breakcrumb") != ""); - CORRADE_VERIFY(viewConfig->get("key") != newKey); - CORRADE_VERIFY(viewConfig->get("depth") != 0); - CORRADE_VERIFY(!viewConfig->hasValue("test_string")); - - // Now Test merge/Overwriting - const std::string mergedKey = "merged_subconfig"; - Configuration::ptr cfgToOverwrite = - cfg->editSubconfig(mergedKey); - - // first set some test values that will be clobbered - cfgToOverwrite->set("key", mergedKey); - cfgToOverwrite->set("depth", 11); - cfgToOverwrite->set("breakcrumb", "1123"); - cfgToOverwrite->set("test_string", "this string will be clobbered"); - // Now add some values that won't be clobbered - cfgToOverwrite->set("myBool", true); - cfgToOverwrite->set("myInt", 10); - cfgToOverwrite->set("myFloatToDouble", 1.2f); - cfgToOverwrite->set("myVec2", Mn::Vector2{1.0, 2.0}); - cfgToOverwrite->set("myVec3", Mn::Vector3{1.0, 2.0, 3.0}); - cfgToOverwrite->set("myVec4", Mn::Vector4{1.0, 2.0, 3.0, 4.0}); - cfgToOverwrite->set("myQuat", Mn::Quaternion{{1.0, 2.0, 3.0}, 0.1}); - cfgToOverwrite->set("myRad", Mn::Rad{1.23}); - cfgToOverwrite->set("myString", "test"); - - // Now overwrite the values from cfgToVerify - cfgToOverwrite->overwriteWithConfig(cfgToVerify); - - CORRADE_VERIFY(cfg->hasSubconfig(mergedKey)); - // Verify all the overwritten values are correct - Configuration::cptr cfgToVerifyOverwrite = cfg->getSubconfigView(mergedKey); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("breakcrumb"), ""); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("test_string"), - "this is an added test string"); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("key"), newKey); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("depth"), 0); - // Verify original non-overwritten values are still present - CORRADE_COMPARE(cfgToVerifyOverwrite->get("myBool"), true); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("myInt"), 10); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("myFloatToDouble"), 1.2f); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("myVec2"), - Mn::Vector2(1.0, 2.0)); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("myVec3"), - Mn::Vector3(1.0, 2.0, 3.0)); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("myVec4"), - Mn::Vector4(1.0, 2.0, 3.0, 4.0)); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("myQuat"), - Mn::Quaternion({1.0, 2.0, 3.0}, 0.1)); - CORRADE_COMPARE(cfgToVerifyOverwrite->get("myRad"), Mn::Rad(1.23)); - - // Verify overwrite performed properly - compareSubconfigs(cfgToVerify, cfgToVerifyOverwrite); - } - -} // CoreTest::TestConfigurationSubconfigFind test - -} // namespace - -CORRADE_TEST_MAIN(CoreTest) diff --git a/src/tests/PhysicsTest.cpp b/src/tests/PhysicsTest.cpp index 81af04e28b..43d0bacc7d 100644 --- a/src/tests/PhysicsTest.cpp +++ b/src/tests/PhysicsTest.cpp @@ -224,7 +224,7 @@ void PhysicsTest::testJoinCompound() { objectTemplate->setJoinCollisionMeshes(true); } objectAttributesManager->registerObject(objectTemplate); - physicsManager_->reset(); + physicsManager_->reset(false); // add and simulate objects int num_objects = 7; @@ -302,7 +302,7 @@ void PhysicsTest::testCollisionBoundingBox() { objectTemplate->setBoundingBoxCollisions(true); } objectAttributesManager->registerObject(objectTemplate); - physicsManager_->reset(); + physicsManager_->reset(false); auto objectWrapper = makeObjectGetWrapper( objectFile, &sceneManager_->getSceneGraph(sceneID_).getDrawables()); @@ -612,7 +612,7 @@ void PhysicsTest::testMotionTypes() { // reset the scene rigidObjectManager_->removeAllObjects(); - physicsManager_->reset(); // time=0 + physicsManager_->reset(false); // time=0 } } } // PhysicsTest::testMotionTypes diff --git a/src_python/habitat_sim/utils/__init__.py b/src_python/habitat_sim/utils/__init__.py index e1c3f2418e..dcd157737f 100755 --- a/src_python/habitat_sim/utils/__init__.py +++ b/src_python/habitat_sim/utils/__init__.py @@ -9,14 +9,9 @@ # TODO @maksymets: remove after habitat-lab/examples/new_actions.py will be # fixed - -from habitat_sim.utils import common, manager_utils, settings, validators, viz_utils -from habitat_sim.utils.common import quat_from_angle_axis, quat_rotate_vector +from habitat_sim.utils import manager_utils, settings, validators, viz_utils __all__ = [ - "quat_from_angle_axis", - "quat_rotate_vector", - "common", "viz_utils", "validators", "settings", diff --git a/src_python/habitat_sim/utils/classes/__init__.py b/src_python/habitat_sim/utils/classes/__init__.py new file mode 100755 index 0000000000..435063cc04 --- /dev/null +++ b/src_python/habitat_sim/utils/classes/__init__.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +from habitat_sim.utils.classes.markersets_editor import MarkerSetsEditor +from habitat_sim.utils.classes.object_editor import ObjectEditor +from habitat_sim.utils.classes.semantic_display import SemanticDisplay + +__all__ = [ + "ObjectEditor", + "MarkerSetsEditor", + "SemanticDisplay", +] diff --git a/src_python/habitat_sim/utils/classes/markersets_editor.py b/src_python/habitat_sim/utils/classes/markersets_editor.py new file mode 100644 index 0000000000..e545410667 --- /dev/null +++ b/src_python/habitat_sim/utils/classes/markersets_editor.py @@ -0,0 +1,436 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import os +from typing import Any, Dict, Set + +import magnum as mn +import numpy as np + +import habitat_sim +from habitat_sim.utils.namespace.hsim_physics import ( + get_obj_from_handle, + get_obj_from_id, +) + + +class MarkerSetsEditor: + def __init__( + self, sim: habitat_sim.simulator.Simulator, task_names_set: Set = None + ): + # Handle default being none + if task_names_set is None: + task_names_set = set() + self.sim = sim + self.markerset_taskset_names = list(task_names_set) + self.update_markersets() + + def update_markersets(self): + """ + Call when created and when a new AO is added or removed + """ + task_names_set = set(self.markerset_taskset_names) + + self.marker_sets_per_obj = self.get_all_markersets() + + # initialize list of possible taskSet names along with those being added specifically + for sub_dict in self.marker_sets_per_obj.values(): + for _obj_handle, MarkerSet in sub_dict.items(): + task_names_set.update(MarkerSet.get_all_taskset_names()) + + # Necessary class-level variables. + self.markerset_taskset_names = list(task_names_set) + # remove trailing s from taskset name for each markerset label prefix + self.markerset_label_prefix = [ + s[:-1] if s.endswith("s") else s for s in self.markerset_taskset_names + ] + + self.current_markerset_taskset_idx = 0 + + self.marker_sets_changed: Dict[str, Dict[str, bool]] = {} + # Hierarchy of colors to match the markerset hierarchy + self.marker_debug_random_colors: Dict[str, Dict[str, Any]] = {} + for sub_key, sub_dict in self.marker_sets_per_obj.items(): + tmp_dict_changed = {} + tmp_dict_colors: Dict[str, Any] = {} + for key in sub_dict: + print(f"subkey : {sub_key} | key : {key}") + tmp_dict_changed[key] = False + tmp_dict_colors[key] = {} + self.marker_sets_changed[sub_key] = tmp_dict_changed + self.marker_debug_random_colors[sub_key] = tmp_dict_colors + + # for debugging + self.glbl_marker_point_dicts_per_obj = self.get_all_global_markers() + + def get_current_taskname(self): + """ + Retrieve the name of the currently used markerset taskname, as specified in the current object's markersets + """ + return self.markerset_taskset_names[self.current_markerset_taskset_idx] + + def cycle_current_taskname(self, forward: bool = True): + mod_val = 1 if forward else -1 + self.current_markerset_taskset_idx = ( + self.current_markerset_taskset_idx + + len(self.markerset_taskset_names) + + mod_val + ) % len(self.markerset_taskset_names) + + def set_current_taskname(self, taskname: str): + if taskname in self.markerset_taskset_names: + self.current_markerset_taskset_idx = self.markerset_taskset_names.index( + taskname + ) + else: + print( + f"Specified taskname {taskname} not valid, so taskname is remaining {self.get_current_taskname()}" + ) + + def get_all_markersets(self): + """ + Get all the markersets defined in the currently loaded objects and articulated objects. + Note : any modified/saved markersets may require their owning Configurations + to be reloaded before this function would expose them. + """ + print("Start getting all markersets") + # marker set cache of existing markersets for all objs in scene, keyed by object name + marker_sets_per_obj = {} + ro_marker_sets = {} + rom = self.sim.get_rigid_object_manager() + obj_dict = rom.get_objects_by_handle_substring("") + for handle, obj in obj_dict.items(): + print(f"setting rigid markersets for {handle}") + ro_marker_sets[handle] = obj.marker_sets + ao_marker_sets = {} + aom = self.sim.get_articulated_object_manager() + ao_obj_dict = aom.get_objects_by_handle_substring("") + for handle, obj in ao_obj_dict.items(): + print(f"setting ao markersets for {handle}") + ao_marker_sets[handle] = obj.marker_sets + print("Done getting all markersets") + + marker_sets_per_obj["ao"] = ao_marker_sets + marker_sets_per_obj["ro"] = ro_marker_sets + + return marker_sets_per_obj + + def place_marker_at_hit_location( + self, hit_info, ao_link_map: Dict[int, int], add_marker: bool + ): + selected_object = None + + # object or ao at hit location. If none, hit stage + obj = get_obj_from_id(self.sim, hit_info.object_id, ao_link_map) + print( + f"Marker : Mouse click object : {hit_info.object_id} : Point : {hit_info.point} " + ) + # TODO these values need to be modifiable + task_set_name = self.get_current_taskname() + marker_set_name = ( + f"{self.markerset_label_prefix[self.current_markerset_taskset_idx]}_000" + ) + if obj is None: + print( + f"Currently can't add a marker to the stage : ID : ({hit_info.object_id})." + ) + # TODO get stage's marker_sets properly + obj_marker_sets = None + obj_handle = "stage" + link_ix = -1 + link_name = "root" + # TODO need to support stage properly including root transformation + local_hit_point = hit_info.point + else: + selected_object = obj + # get a reference to the object/ao 's markerSets + obj_marker_sets = obj.marker_sets + obj_handle = obj.handle + if obj.is_articulated: + obj_type = "articulated object" + # this is an ArticulatedLink, so we can add markers' + link_ix = obj.link_object_ids[hit_info.object_id] + link_name = obj.get_link_name(link_ix) + obj_type_key = "ao" + + else: + obj_type = "rigid object" + # this is an ArticulatedLink, so we can add markers' + link_ix = -1 + link_name = "root" + obj_type_key = "ro" + + local_hit_point_list = obj.transform_world_pts_to_local( + [hit_info.point], link_ix + ) + # get location in local space + local_hit_point = local_hit_point_list[0] + + print( + f"Marker on this {obj_type} : {obj_handle} link Idx : {link_ix} : link name : {link_name} world point : {hit_info.point} local_hit_point : {local_hit_point}" + ) + + if obj_marker_sets is not None: + # add marker if left button clicked + if add_marker: + # if the desired hierarchy does not exist, create it + if not obj_marker_sets.has_task_link_markerset( + task_set_name, link_name, marker_set_name + ): + obj_marker_sets.init_task_link_markerset( + task_set_name, link_name, marker_set_name + ) + # get points for current task_set i.e. ("faucets"), link_name, marker_set_name i.e.("faucet_000") + curr_markers = obj_marker_sets.get_task_link_markerset_points( + task_set_name, link_name, marker_set_name + ) + # add point to list + curr_markers.append(local_hit_point) + # save list + obj_marker_sets.set_task_link_markerset_points( + task_set_name, link_name, marker_set_name, curr_markers + ) + else: + # right click is remove marker + print( + f"About to check obj {obj_handle} if it has points in MarkerSet : {marker_set_name}, LinkSet :{link_name}, TaskSet :{task_set_name} so removal aborted." + ) + if obj_marker_sets.has_task_link_markerset( + task_set_name, link_name, marker_set_name + ): + # Non-empty markerset so find closest point to target and delete + curr_markers = obj_marker_sets.get_task_link_markerset_points( + task_set_name, link_name, marker_set_name + ) + # go through every point to find closest + closest_marker_index = None + closest_marker_dist = 999999 + for m_idx in range(len(curr_markers)): + m_dist = (local_hit_point - curr_markers[m_idx]).length() + if m_dist < closest_marker_dist: + closest_marker_dist = m_dist + closest_marker_index = m_idx + if closest_marker_index is not None: + del curr_markers[closest_marker_index] + # save new list + obj_marker_sets.set_task_link_markerset_points( + task_set_name, link_name, marker_set_name, curr_markers + ) + else: + print( + f"There are no points in MarkerSet : {marker_set_name}, LinkSet :{link_name}, TaskSet :{task_set_name} so removal aborted." + ) + self.marker_sets_per_obj[obj_type_key][obj_handle] = obj_marker_sets + self.marker_sets_changed[obj_type_key][obj_handle] = True + self.save_markerset_attributes(obj) + return selected_object + + def draw_marker_sets_debug( + self, debug_line_render: Any, camera_position: mn.Vector3 + ) -> None: + """ + Draw the global state of all configured marker sets. + """ + for obj_type_key, sub_dict in self.marker_sets_per_obj.items(): + color_dict = self.marker_debug_random_colors[obj_type_key] + for obj_handle, obj_markerset in sub_dict.items(): + marker_points_dict = obj_markerset.get_all_marker_points() + if obj_markerset.num_tasksets > 0: + obj = get_obj_from_handle(self.sim, obj_handle) + for task_name, task_set_dict in marker_points_dict.items(): + if task_name not in color_dict[obj_handle]: + color_dict[obj_handle][task_name] = {} + for link_name, link_set_dict in task_set_dict.items(): + if link_name not in color_dict[obj_handle][task_name]: + color_dict[obj_handle][task_name][link_name] = {} + if link_name in ["root", "body"]: + link_id = -1 + else: + link_id = obj.get_link_id_from_name(link_name) + + for ( + markerset_name, + marker_pts_list, + ) in link_set_dict.items(): + # print(f"markerset_name : {markerset_name} : marker_pts_list : {marker_pts_list} type : {type(marker_pts_list)} : len : {len(marker_pts_list)}") + if ( + markerset_name + not in color_dict[obj_handle][task_name][link_name] + ): + color_dict[obj_handle][task_name][link_name][ + markerset_name + ] = mn.Color4(mn.Vector3(np.random.random(3))) + marker_set_color = color_dict[obj_handle][task_name][ + link_name + ][markerset_name] + global_points = obj.transform_local_pts_to_world( + marker_pts_list, link_id + ) + for global_marker_pos in global_points: + debug_line_render.draw_circle( + translation=global_marker_pos, + radius=0.005, + color=marker_set_color, + normal=camera_position - global_marker_pos, + ) + + def save_all_dirty_markersets(self) -> None: + # save config for object handle's markersets + for subdict in self.marker_sets_changed.values(): + for obj_handle, is_dirty in subdict.items(): + if is_dirty: + obj = get_obj_from_handle(self.sim, obj_handle) + self.save_markerset_attributes(obj) + + def save_markerset_attributes(self, obj) -> None: + """ + Modify the attributes for the passed object to include the + currently edited markersets and save those attributes to disk + """ + # get the name of the attrs used to initialize the object + obj_init_attr_handle = obj.creation_attributes.handle + + if obj.is_articulated: + # save AO config + attrMgr = self.sim.metadata_mediator.ao_template_manager + subdict = "ao" + else: + # save obj config + attrMgr = self.sim.metadata_mediator.object_template_manager + subdict = "ro" + # get copy of initialization attributes as they were in manager, + # unmodified by scene instance values such as scale + init_attrs = attrMgr.get_template_by_handle(obj_init_attr_handle) + # TEMP TODO Remove this when fixed in Simulator + # Clean up sub-dirs being added to asset handles. + if obj.is_articulated: + init_attrs.urdf_filepath = init_attrs.urdf_filepath.split(os.sep)[-1] + init_attrs.render_asset_handle = init_attrs.render_asset_handle.split( + os.sep + )[-1] + else: + init_attrs.render_asset_handle = init_attrs.render_asset_handle.split( + os.sep + )[-1] + init_attrs.collision_asset_handle = init_attrs.collision_asset_handle.split( + os.sep + )[-1] + # put edited subconfig into initial attributes' markersets + markersets = init_attrs.get_marker_sets() + # manually copying because the markersets type is getting lost from markersets + edited_markersets = self.marker_sets_per_obj[subdict][obj.handle] + if edited_markersets.top_level_num_entries == 0: + # if all subconfigs of edited_markersets are gone, clear out those in + # markersets ref within attributes. + for subconfig_key in markersets.get_subconfig_keys(): + markersets.remove_subconfig(subconfig_key) + else: + # Copy subconfigs from local copy of markersets to init attributes' copy + for subconfig_key in edited_markersets.get_subconfig_keys(): + markersets.save_subconfig( + subconfig_key, edited_markersets.get_subconfig(subconfig_key) + ) + + # reregister template + attrMgr.register_template(init_attrs, init_attrs.handle, True) + # save to original location - uses saved location in attributes + attrMgr.save_template_by_handle(init_attrs.handle, True) + # clear out dirty flag + self.marker_sets_changed[subdict][obj.handle] = False + + def get_all_global_markers(self): + """ + Debug function. Get all markers in global space, in nested hierarchy + """ + + def get_points_as_global(obj, marker_points_dict): + new_markerset_dict = {} + # for every task + for task_name, task_dict in marker_points_dict.items(): + new_task_dict = {} + # for every link + for link_name, link_dict in task_dict.items(): + if link_name in ["root", "body"]: + link_id = -1 + else: + # articulated object + link_id = obj.get_link_id_from_name(link_name) + new_link_dict = {} + # for every markerset + for subset, markers_list in link_dict.items(): + new_markers_list = obj.transform_local_pts_to_world( + markers_list, link_id + ) + new_link_dict[subset] = new_markers_list + new_task_dict[link_name] = new_link_dict + new_markerset_dict[task_name] = new_task_dict + return new_markerset_dict + + # marker set cache of existing markersets for all objs in scene, keyed by object name + marker_set_global_dicts_per_obj = {} + marker_set_dict_ao = {} + aom = self.sim.get_articulated_object_manager() + ao_obj_dict = aom.get_objects_by_handle_substring("") + for handle, obj in ao_obj_dict.items(): + marker_set_dict_ao[handle] = get_points_as_global( + obj, obj.marker_sets.get_all_marker_points() + ) + marker_set_dict_ro = {} + rom = self.sim.get_rigid_object_manager() + obj_dict = rom.get_objects_by_handle_substring("") + for handle, obj in obj_dict.items(): + marker_set_dict_ro[handle] = get_points_as_global( + obj, obj.marker_sets.get_all_marker_points() + ) + marker_set_global_dicts_per_obj["ao"] = marker_set_dict_ao + marker_set_global_dicts_per_obj["ro"] = marker_set_dict_ro + return marker_set_global_dicts_per_obj + + def _draw_markersets_glbl_debug_objtype( + self, obj_type_key: str, debug_line_render: Any, camera_position: mn.Vector3 + ) -> None: + obj_dict = self.glbl_marker_point_dicts_per_obj[obj_type_key] + color_dict = self.marker_debug_random_colors[obj_type_key] + for ( + obj_handle, + marker_points_dict, + ) in obj_dict.items(): + for task_name, task_set_dict in marker_points_dict.items(): + if task_name not in color_dict[obj_handle]: + color_dict[obj_handle][task_name] = {} + for link_name, link_set_dict in task_set_dict.items(): + if link_name not in color_dict[obj_handle][task_name]: + color_dict[obj_handle][task_name][link_name] = {} + + for markerset_name, global_points in link_set_dict.items(): + # print(f"markerset_name : {markerset_name} : marker_pts_list : {marker_pts_list} type : {type(marker_pts_list)} : len : {len(marker_pts_list)}") + if ( + markerset_name + not in color_dict[obj_handle][task_name][link_name] + ): + color_dict[obj_handle][task_name][link_name][ + markerset_name + ] = mn.Color4(mn.Vector3(np.random.random(3))) + marker_set_color = color_dict[obj_handle][task_name][link_name][ + markerset_name + ] + for global_marker_pos in global_points: + debug_line_render.draw_circle( + translation=global_marker_pos, + radius=0.005, + color=marker_set_color, + normal=camera_position - global_marker_pos, + ) + + def draw_markersets_glbl_debug( + self, debug_line_render: Any, camera_position: mn.Vector3 + ) -> None: + self._draw_markersets_glbl_debug_objtype( + "ao", debug_line_render=debug_line_render, camera_position=camera_position + ) + self._draw_markersets_glbl_debug_objtype( + "ro", debug_line_render=debug_line_render, camera_position=camera_position + ) diff --git a/src_python/habitat_sim/utils/classes/object_editor.py b/src_python/habitat_sim/utils/classes/object_editor.py new file mode 100644 index 0000000000..b2182f9da4 --- /dev/null +++ b/src_python/habitat_sim/utils/classes/object_editor.py @@ -0,0 +1,969 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + + +from collections import defaultdict +from enum import Enum +from typing import Any, Dict, List, Optional + +import magnum as mn +from numpy import pi + +import habitat_sim +from habitat_sim import physics as HSim_Phys +from habitat_sim.physics import MotionType as HSim_Phys_MT +from habitat_sim.utils.namespace.hsim_physics import get_ao_root_bb, open_link + + +# Class to control editing objects +# Create an instance and then map various keyboard keys/interactive inputs to appropriate functions. +# Be sure to always specify selected object when it changes. +class ObjectEditor: + # Describe edit type + class EditMode(Enum): + MOVE = 0 + ROTATE = 1 + NUM_VALS = 2 # last value + + EDIT_MODE_NAMES = ["Move object", "Rotate object"] + + # Describe edit distance values + class DistanceMode(Enum): + TINY = 0 + VERY_SMALL = 1 + SMALL = 2 + MEDIUM = 3 + LARGE = 4 + HUGE = 5 + NUM_VALS = 6 + + # What kind of objects to display boxes around + class ObjectTypeToDraw(Enum): + NONE = 0 + ARTICULATED = 1 + RIGID = 2 + BOTH = 3 + NUM_VALS = 4 + + OBJECT_TYPE_NAMES = ["", "Articulated", "Rigid", "Both"] + + # distance values in m + 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, 10.0, 30.0, 45.0, 60.0, 90.0] + # 1 radian + BASE_EDIT_ROT_AMT = pi / 180.0 + + def __init__(self, sim: habitat_sim.simulator.Simulator): + self.sim = sim + # Set up object and edit collections/caches + self._init_obj_caches() + # Edit mode + self.curr_edit_mode = ObjectEditor.EditMode.MOVE + # Edit distance/amount + self.curr_edit_multiplier = ObjectEditor.DistanceMode.VERY_SMALL + # Type of objects to draw highlight aabb boxes around + self.obj_type_to_draw = ObjectEditor.ObjectTypeToDraw.NONE + # Set initial values + self.set_edit_vals() + + def _init_obj_caches(self): + # Internal: Dict of currently selected object ids to index in + self._sel_obj_ids: Dict[int, int] = {} + # list of current objects selected. Last object in list is "target" object + self.sel_objs: List[Any] = [] + # Complete list of per-objec transformation edits, for undo chaining, + # keyed by object id, value is before and after transform + self.obj_transform_edits: Dict[ + int, List[tuple[mn.Matrix4, mn.Matrix4, bool]] + ] = defaultdict(list) + # Dictionary by object id of transformation when object was most recently saved + self.obj_last_save_transform: Dict[int, mn.Matrix4] = {} + + # Complete list of undone transformation edits, for redo chaining, + # keyed by object id, value is before and after transform. + # Cleared when any future edits are performed. + self.obj_transform_undone_edits: Dict[ + int, List[tuple[mn.Matrix4, mn.Matrix4, bool]] + ] = defaultdict(list) + + # Cache removed objects in dictionary + # These objects should be hidden/moved out of vieew until the scene is + # saved, when they should be actually removed (to allow for undo). + # First key is whether they are articulated or not, value is dict with key == object id, value is object, + self._removed_objs: Dict[bool, Dict[int, Any]] = defaultdict(dict) + # Initialize a flag tracking if the scene is dirty or not + self.modified_scene: bool = False + + def set_edit_mode_rotate(self): + self.curr_edit_mode = ObjectEditor.EditMode.ROTATE + + def set_edit_vals(self): + # Set current scene object edit values for translation and rotation + # 1 cm * multiplier + self.edit_translation_dist = ObjectEditor.DISTANCE_MODE_VALS[ + self.curr_edit_multiplier.value + ] + # 1 radian * multiplier + self.edit_rotation_amt = ( + ObjectEditor.BASE_EDIT_ROT_AMT + * ObjectEditor.ROTATION_MULT_VALS[self.curr_edit_multiplier.value] + ) + + def get_target_sel_obj(self): + """ + Retrieve the primary/target selected object. If none then will return none + """ + if len(self.sel_objs) == 0: + return None + return self.sel_objs[-1] + + def edit_disp_str(self): + """ + Specify display quantities for editing + """ + + edit_mode_string = ObjectEditor.EDIT_MODE_NAMES[self.curr_edit_mode.value] + + dist_mode_substr = ( + f"Translation: {self.edit_translation_dist}m" + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE + else f"Rotation:{ObjectEditor.ROTATION_MULT_VALS[self.curr_edit_multiplier.value]} deg " + ) + edit_distance_mode_string = f"{dist_mode_substr}" + obj_str = self.edit_obj_disp_str() + obj_type_disp_str = ( + "" + if self.obj_type_to_draw == ObjectEditor.ObjectTypeToDraw.NONE + else f"\nObject Types Being Displayed :{ObjectEditor.OBJECT_TYPE_NAMES[self.obj_type_to_draw.value]}" + ) + disp_str = f"""Edit Mode: {edit_mode_string} +Edit Value: {edit_distance_mode_string} +Scene Is Modified: {self.modified_scene} +Num Sel Objs: {len(self.sel_objs)}{obj_str}{obj_type_disp_str} + """ + return disp_str + + def edit_obj_disp_str(self): + """ + Specify primary selected object display quantities + """ + if len(self.sel_objs) == 0: + return "" + sel_obj = self.sel_objs[-1] + if sel_obj.is_articulated: + tar_str = ( + f"Articulated Object : {sel_obj.handle} with {sel_obj.num_links} links." + ) + else: + tar_str = f"Rigid Object : {sel_obj.handle}" + return f"\nTarget Object is {tar_str}" + + def _clear_sel_objs(self): + """ + Internal: clear object selection structure(s) + """ + self._sel_obj_ids.clear() + self.sel_objs.clear() + + def _add_obj_to_sel(self, obj): + """ + Internal : add object to selection structure(s) + """ + self._sel_obj_ids[obj.object_id] = len(self.sel_objs) + # Add most recently selected object to list of selected objects + self.sel_objs.append(obj) + + def _remove_obj_from_sel(self, obj): + """ + Internal : remove object from selection structure(s) + """ + new_objs: List[Any] = [] + # Rebuild selected object structures without the removed object + self._sel_obj_ids.clear() + for old_obj in self.sel_objs: + if old_obj.object_id != obj.object_id: + self._sel_obj_ids[old_obj.object_id] = len(new_objs) + new_objs.append(old_obj) + self.sel_objs = new_objs + + def set_sel_obj(self, obj): + """ + Set the selected objects to just be the passed obj + """ + self._clear_sel_objs() + if obj is not None: + self._add_obj_to_sel(obj) + + def toggle_sel_obj(self, obj): + """ + Remove or add the passed object to the selected objects dict, depending on whether it is present, or not. + """ + if obj is not None: + if obj.object_id in self._sel_obj_ids: + # Remove object from obj selected dict + self._remove_obj_from_sel(obj) + else: + # Add object to selected dict + self._add_obj_to_sel(obj) + + def set_ao_joint_states( + self, do_open: bool, selected: bool, agent_name: str = "hab_spot" + ): + """ + Set either the selected articulated object states to either fully open or fully closed, or all the articulated object states (except for any robots) + """ + if selected: + # Only selected objs if they are articulated + ao_objs = [ao for ao in self.sel_objs if ao.is_articulated] + else: + # all articulated objs that do not contain agent's name + ao_objs = ( + self.sim.get_articulated_object_manager() + .get_objects_by_handle_substring(search_str=agent_name, contains=False) + .values() + ) + + if do_open: + # Open AOs + for ao in ao_objs: + # open the selected receptacle(s) + for link_ix in ao.get_link_ids(): + if ao.get_link_joint_type(link_ix) in [ + HSim_Phys.JointType.Prismatic, + HSim_Phys.JointType.Revolute, + ]: + open_link(ao, link_ix) + else: + # Close AOs + for ao in ao_objs: + 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 _set_scene_dirty(self): + """ + Set whether the scene is currently modified from saved version or + not. If there are objects to be deleted or cached transformations + in the undo stack, this flag should be true. + """ + # Set scene to be modified if any aos or rigids have been marked + # for deletion, or any objects have been transformed + self.modified_scene = (len(self._removed_objs[True]) > 0) or ( + len(self._removed_objs[False]) > 0 + ) + + # only check transforms if still false + if not self.modified_scene: + # check all object transformations match most recent edit transform + for obj_id, transform_list in self.obj_transform_edits.items(): + if (obj_id not in self.obj_last_save_transform) or ( + len(transform_list) == 0 + ): + continue + curr_transform = transform_list[-1][1] + if curr_transform != self.obj_last_save_transform[obj_id]: + self.modified_scene = True + + def _move_one_object( + self, + obj, + navmesh_dirty: bool, + translation: Optional[mn.Vector3] = None, + rotation: Optional[mn.Quaternion] = None, + removal: bool = False, + ) -> bool: + """ + Internal. Move a single object with a given modification and save the resulting state to the buffer. + Returns whether the navmesh should be rebuilt due to an object changing position, or previous edits. + """ + if obj is None: + print("No object is selected so ignoring move request") + return navmesh_dirty + action_str = ( + f"{'Articulated' if obj.is_articulated else 'Rigid'} Object {obj.handle}" + ) + # If object is marked for deletion, don't allow it to be further moved + if obj.object_id in self._removed_objs[obj.is_articulated]: + print( + f"{action_str} already marked for deletion so cannot be moved. Restore object to change its transformation." + ) + return navmesh_dirty + # Move object if transforms exist + if translation is not None or rotation is not None: + orig_transform = obj.transformation + # First time save of original transformation for objects being moved + if obj.object_id not in self.obj_last_save_transform: + self.obj_last_save_transform[obj.object_id] = orig_transform + orig_mt = obj.motion_type + obj.motion_type = HSim_Phys_MT.KINEMATIC + if translation is not None: + obj.translation = obj.translation + translation + action_str = f"{action_str} translated to {obj.translation};" + if rotation is not None: + obj.rotation = rotation * obj.rotation + action_str = f"{action_str} rotated to {obj.rotation};" + print(action_str) + obj.motion_type = orig_mt + # Save transformation for potential undoing later + trans_tuple = (orig_transform, obj.transformation, removal) + self.obj_transform_edits[obj.object_id].append(trans_tuple) + # Clear entries for redo since we have a new edit + self.obj_transform_undone_edits[obj.object_id] = [] + # Set whether scene has been modified or not + self._set_scene_dirty() + return True + return navmesh_dirty + + def move_sel_objects( + self, + navmesh_dirty: bool, + translation: Optional[mn.Vector3] = None, + rotation: Optional[mn.Quaternion] = None, + removal: bool = False, + ) -> bool: + """ + Move all selected objects with a given modification and save the resulting state to the buffer. + Returns whether the navmesh should be rebuilt due to an object's transformation changing, or previous edits. + """ + for obj in self.sel_objs: + new_navmesh_dirty = self._move_one_object( + obj, + navmesh_dirty, + translation=translation, + rotation=rotation, + removal=removal, + ) + navmesh_dirty = new_navmesh_dirty or navmesh_dirty + return navmesh_dirty + + def _remove_obj(self, obj): + """ + Move and mark the passed object for removal from the scene. + """ + # move selected object outside of direct render area -20 m below current location + translation = mn.Vector3(0.0, -20.0, 0.0) + # ignore navmesh result; removal always recomputes + self._move_one_object(obj, True, translation=translation, removal=True) + # record removed object for eventual deletion upon scene save + self._removed_objs[obj.is_articulated][obj.object_id] = obj + + def _restore_obj(self, obj): + """ + Restore the passed object from the removal queue + """ + # move selected object back to where it was before - back 20m up + translation = mn.Vector3(0.0, 20.0, 0.0) + # remove object from deletion record + self._removed_objs[obj.is_articulated].pop(obj.object_id, None) + # ignore navmesh result; restoration always recomputes + self._move_one_object(obj, True, translation=translation, removal=False) + + def remove_sel_objects(self): + """ + 'Removes' all selected objects from the scene by hiding them and putting them in queue to be deleted on + scene save update. Returns list of the handles of all the objects removed if successful + + Note : removal is not permanent unless scene is saved. + """ + removed_obj_handles = [] + for obj in self.sel_objs: + if obj is None: + continue + self._remove_obj(obj) + print( + f"Moved {obj.handle} out of view and marked for removal. Removal becomes permanent when scene is saved." + ) + # record handle of removed objects, for return + removed_obj_handles.append(obj.handle) + # unselect all objects, since they were all 'removed' + self._clear_sel_objs() + # retain all object selected transformations. + return removed_obj_handles + + def restore_removed_objects(self): + """ + Undo removals that have not been saved yet via scene instance. Will put object back where it was before marking it for removal + """ + restored_obj_handles = [] + obj_rem_dict = self._removed_objs[True] + removed_obj_keys = list(obj_rem_dict.keys()) + for obj_id in removed_obj_keys: + obj = obj_rem_dict.pop(obj_id, None) + if obj is not None: + self._restore_obj(obj) + restored_obj_handles.append(obj.handle) + obj_rem_dict = self._removed_objs[False] + removed_obj_keys = list(obj_rem_dict.keys()) + for obj_id in removed_obj_keys: + obj = obj_rem_dict.pop(obj_id, None) + if obj is not None: + self._restore_obj(obj) + restored_obj_handles.append(obj.handle) + + # Set whether scene is still considered modified/'dirty' + self._set_scene_dirty() + return restored_obj_handles + + def delete_removed_objs(self): + """ + Delete all the objects in the scene marked for removal. Call before saving a scene instance. + """ + ao_removed_objs = self._removed_objs[True] + if len(ao_removed_objs) > 0: + ao_mgr = self.sim.get_articulated_object_manager() + for obj_id in ao_removed_objs: + ao_mgr.remove_object_by_id(obj_id) + # Get rid of all recorded transforms of specified object + self.obj_transform_edits.pop(obj_id, None) + ro_removed_objs = self._removed_objs[False] + if len(ro_removed_objs) > 0: + ro_mgr = self.sim.get_rigid_object_manager() + for obj_id in ro_removed_objs: + ro_mgr.remove_object_by_id(obj_id) + # Get rid of all recorded transforms of specified object + self.obj_transform_edits.pop(obj_id, None) + # Set whether scene is still considered modified/'dirty' + self._set_scene_dirty() + + def _undo_obj_edit(self, obj, transform_tuple: tuple[mn.Matrix4, mn.Matrix4, bool]): + """ + Changes the object's current transformation to the passed, previous transformation (in idx 0). + Different than a move, only called by undo/redo procedure + """ + old_transform = transform_tuple[0] + orig_mt = obj.motion_type + obj.motion_type = HSim_Phys_MT.KINEMATIC + obj.transformation = old_transform + obj.motion_type = orig_mt + + def redo_sel_edits(self): + """ + Redo edits that have been undone on all currently selected objects one step + """ + if len(self.sel_objs) == 0: + return + # For every object in selected object + for obj in self.sel_objs: + obj_id = obj.object_id + # Verify there are transforms to redo for this object + if len(self.obj_transform_undone_edits[obj_id]) > 0: + # Last undo state is last element in transforms list + # In tuple idxs : 0 : previous transform, 1 : current transform, 2 : whether was a removal op + # Retrieve and remove last undo + transform_tuple = self.obj_transform_undone_edits[obj_id].pop() + if len(self.obj_transform_undone_edits[obj_id]) == 0: + self.obj_transform_undone_edits.pop(obj_id, None) + # If this had been a removal that had been undone, redo removal + remove_str = "" + if transform_tuple[2]: + # Restore object to removal queue for eventual deletion upon scene save + self._removed_objs[obj.is_articulated][obj.object_id] = obj + remove_str = ", being remarked for removal," + self._undo_obj_edit(obj, transform_tuple) + # Save transformation tuple for subsequent undoing + # Swap order of transforms since they were redon, for potential undo + undo_tuple = ( + transform_tuple[1], + transform_tuple[0], + transform_tuple[2], + ) + self.obj_transform_edits[obj_id].append(undo_tuple) + print( + f"REDO : Sel Obj : {obj.handle} : Current object{remove_str} transformation : \n{transform_tuple[1]}\nReplaced by saved transformation : \n{transform_tuple[0]}" + ) + # Set whether scene is still considered modified/'dirty' + self._set_scene_dirty() + + def undo_sel_edits(self): + """ + Undo the edits that have been performed on all the currently selected objects one step, (TODO including removal marks) + """ + if len(self.sel_objs) == 0: + return + # For every object in selected object + for obj in self.sel_objs: + obj_id = obj.object_id + # Verify there are transforms to undo for this object + if len(self.obj_transform_edits[obj_id]) > 0: + # Last edit state is last element in transforms list + # In tuple idxs : 0 : previous transform, 1 : current transform, 2 : whether was a removal op + # Retrieve and remove last edit + transform_tuple = self.obj_transform_edits[obj_id].pop() + # If all object edits have been removed, also remove entry + if len(self.obj_transform_edits[obj_id]) == 0: + self.obj_transform_edits.pop(obj_id, None) + # If this was a removal, remove object from removal queue + remove_str = "" + if transform_tuple[2]: + # Remove object from removal queue if there - undo removal + self._removed_objs[obj.is_articulated].pop(obj_id, None) + remove_str = ", being restored from removal list," + self._undo_obj_edit(obj, transform_tuple) + # Save transformation tuple for redoing + # Swap order of transforms since they were undone, for potential redo + redo_tuple = ( + transform_tuple[1], + transform_tuple[0], + transform_tuple[2], + ) + self.obj_transform_undone_edits[obj_id].append(redo_tuple) + print( + f"UNDO : Sel Obj : {obj.handle} : Current object{remove_str} transformation : \n{transform_tuple[1]}\nReplaced by saved transformation : \n{transform_tuple[0]}" + ) + # Set whether scene is still considered modified/'dirty' + self._set_scene_dirty() + + def select_all_matching_objects(self, only_matches: bool): + """ + Selects all objects matching currently selected object (or first object selected) + only_matches : only select objects that match type of first selected object (deselects all others) + """ + if len(self.sel_objs) == 0: + return + # primary object is always at idx -1 + match_obj = self.sel_objs[-1] + obj_is_articulated = match_obj.is_articulated + if only_matches: + # clear out existing objects + self._clear_sel_objs() + + attr_mgr = ( + self.sim.get_articulated_object_manager() + if obj_is_articulated + else self.sim.get_rigid_object_manager() + ) + match_obj_handle = match_obj.handle.split("_:")[0] + new_sel_objs_dict = attr_mgr.get_objects_by_handle_substring( + search_str=match_obj_handle, contains=True + ) + for obj in new_sel_objs_dict.values(): + self._add_obj_to_sel(obj) + # reset match_obj as selected object by first deleting and then re-adding + self.toggle_sel_obj(match_obj) + self.toggle_sel_obj(match_obj) + + def match_dim_sel_objects( + self, + navmesh_dirty: bool, + new_val: float, + axis: mn.Vector3, + ) -> bool: + """ + Set all selected objects to have the same specified translation dimension value. + new_val : new value to set the location of the object + axis : the dimension's axis to match the value of + """ + trans_vec = new_val * axis + for obj in self.sel_objs: + obj_mod_vec = obj.translation.projected(axis) + new_navmesh_dirty = self._move_one_object( + obj, + navmesh_dirty, + translation=trans_vec - obj_mod_vec, + removal=False, + ) + navmesh_dirty = new_navmesh_dirty or navmesh_dirty + return navmesh_dirty + + def match_x_dim(self, navmesh_dirty: bool) -> bool: + """ + All selected objects should match specified target object's x value + """ + if len(self.sel_objs) == 0: + return None + match_val = self.sel_objs[-1].translation.x + return self.match_dim_sel_objects(navmesh_dirty, match_val, mn.Vector3.x_axis()) + + def match_y_dim(self, navmesh_dirty: bool) -> bool: + """ + All selected objects should match specified target object's y value + """ + if len(self.sel_objs) == 0: + return None + match_val = self.sel_objs[-1].translation.y + return self.match_dim_sel_objects(navmesh_dirty, match_val, mn.Vector3.y_axis()) + + def match_z_dim(self, navmesh_dirty: bool) -> bool: + """ + All selected objects should match specified target object's z value + """ + if len(self.sel_objs) == 0: + return None + match_val = self.sel_objs[-1].translation.z + return self.match_dim_sel_objects(navmesh_dirty, match_val, mn.Vector3.z_axis()) + + def match_orientation(self, navmesh_dirty: bool) -> bool: + """ + All selected objects should match specified target object's orientation + """ + if len(self.sel_objs) == 0: + return None + match_rotation = self.sel_objs[-1].rotation + local_navmesh_dirty = False + for obj in self.sel_objs: + obj_mod_rot = match_rotation * obj.rotation.inverted() + local_navmesh_dirty = self._move_one_object( + obj, + navmesh_dirty, + rotation=obj_mod_rot, + removal=False, + ) + navmesh_dirty = navmesh_dirty or local_navmesh_dirty + return navmesh_dirty + + def edit_left(self, navmesh_dirty: bool) -> bool: + """ + Edit selected objects for left key input + """ + # if movement mode + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE: + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + translation=mn.Vector3.x_axis() * self.edit_translation_dist, + ) + # if rotation mode : rotate around y axis + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + rotation=mn.Quaternion.rotation( + mn.Rad(self.edit_rotation_amt), mn.Vector3.y_axis() + ), + ) + + def edit_right(self, navmesh_dirty: bool): + """ + Edit selected objects for right key input + """ + # if movement mode + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE: + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + translation=-mn.Vector3.x_axis() * self.edit_translation_dist, + ) + # if rotation mode : rotate around y axis + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + rotation=mn.Quaternion.rotation( + -mn.Rad(self.edit_rotation_amt), mn.Vector3.y_axis() + ), + ) + return navmesh_dirty + + def edit_up(self, navmesh_dirty: bool, toggle: bool): + """ + Edit selected objects for up key input + """ + # if movement mode + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE: + trans_axis = mn.Vector3.y_axis() if toggle else mn.Vector3.z_axis() + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + translation=trans_axis * self.edit_translation_dist, + ) + + # if rotation mode : rotate around x or z axis + rot_axis = mn.Vector3.x_axis() if toggle else mn.Vector3.z_axis() + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + rotation=mn.Quaternion.rotation(mn.Rad(self.edit_rotation_amt), rot_axis), + ) + + def edit_down(self, navmesh_dirty: bool, toggle: bool): + """ + Edit selected objects for down key input + """ + # if movement mode + if self.curr_edit_mode == ObjectEditor.EditMode.MOVE: + trans_axis = -mn.Vector3.y_axis() if toggle else -mn.Vector3.z_axis() + + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + translation=trans_axis * self.edit_translation_dist, + ) + # if rotation mode : rotate around x or z axis + rot_axis = mn.Vector3.x_axis() if toggle else mn.Vector3.z_axis() + return self.move_sel_objects( + navmesh_dirty=navmesh_dirty, + rotation=mn.Quaternion.rotation(-mn.Rad(self.edit_rotation_amt), rot_axis), + ) + + def save_current_scene(self): + if self.modified_scene: + # update scene with removals before saving + self.delete_removed_objs() + + # clear out cache of removed objects by resetting dictionary + self._removed_objs = defaultdict(dict) + # Reset all AOs to be 0 + self.set_ao_joint_states(do_open=False, selected=False) + # Save current scene + self.sim.save_current_scene_config(overwrite=True) + # Specify most recent edits for each object that has an undo queue + self.obj_last_save_transform = {} + obj_ids = list(self.obj_transform_edits.keys()) + for obj_id in obj_ids: + transform_list = self.obj_transform_edits[obj_id] + if len(transform_list) == 0: + # if transform list is empty, delete it and skip + self.obj_transform_edits.pop(obj_id, None) + continue + self.obj_last_save_transform[obj_id] = transform_list[-1][1] + + # Clear edited flag + self.modified_scene = False + # + print("Saved modified scene instance JSON to original location.") + + def load_from_substring( + self, navmesh_dirty: bool, obj_substring: str, build_loc: mn.Vector3 + ): + sim = self.sim + mm = sim.metadata_mediator + template_mgr = mm.ao_template_manager + template_handles = template_mgr.get_template_handles(obj_substring) + build_ao = False + print(f"Attempting to find {obj_substring} as an articulated object") + if len(template_handles) == 1: + print(f"{obj_substring} found as an AO!") + # Specific AO template found + obj_mgr = sim.get_articulated_object_manager() + base_motion_type = HSim_Phys_MT.DYNAMIC + build_ao = True + else: + print(f"Attempting to find {obj_substring} as a rigid object instead") + template_mgr = mm.object_template_manager + template_handles = template_mgr.get_template_handles(obj_substring) + if len(template_handles) != 1: + print( + f"No distinct Rigid or Articulated Object handle found matching substring: '{obj_substring}'. Aborting" + ) + return [], navmesh_dirty + print(f"{obj_substring} found as an RO!") + # Specific Rigid template found + obj_mgr = sim.get_rigid_object_manager() + base_motion_type = HSim_Phys_MT.STATIC + + obj_temp_handle = template_handles[0] + # Build an object using obj_temp_handle, getting template from attr_mgr and object manager obj_mgr + temp = template_mgr.get_template_by_handle(obj_temp_handle) + + if build_ao: + obj_type = "Articulated" + temp.base_type = "FIXED" + template_mgr.register_template(temp) + new_obj = obj_mgr.add_articulated_object_by_template_handle(obj_temp_handle) + else: + # If any changes to template, put them here and re-register template + # template_mgr.register_template(temp) + obj_type = "Rigid" + new_obj = obj_mgr.add_object_by_template_handle(obj_temp_handle) + + if new_obj is not None: + # set new object location to be above location of selected object + new_obj.motion_type = base_motion_type + self.set_sel_obj(new_obj) + # move new object to appropriate location + new_navmesh_dirty = self._move_one_object( + new_obj, navmesh_dirty, translation=build_loc + ) + navmesh_dirty = new_navmesh_dirty or navmesh_dirty + else: + print( + f"Failed to load/create {obj_type} Object from template named {obj_temp_handle}." + ) + # creation failing would have its own message + return [], navmesh_dirty + return [new_obj], navmesh_dirty + + def build_objects(self, navmesh_dirty: bool, build_loc: mn.Vector3): + """ + Make a copy of the selected object(s), or load a named item at some distance away + """ + sim = self.sim + if len(self.sel_objs) > 0: + # Copy all selected objects + res_objs = [] + for obj in self.sel_objs: + # Duplicate object via object ID + if obj.is_articulated: + # duplicate articulated object + new_obj = sim.get_articulated_object_manager().duplicate_articulated_object_by_id( + obj.object_id + ) + else: + # duplicate rigid object + new_obj = sim.get_rigid_object_manager().duplicate_object_by_id( + obj.object_id + ) + + if new_obj is not None: + # set new object location to be above location of copied object + new_obj_translation = mn.Vector3(0.0, 1.0, 0.0) + # set new object rotation to match copied object's rotation + new_obj_rotation = obj.rotation * new_obj.rotation.inverted() + new_obj.motion_type = obj.motion_type + # move new object to appropriate location + new_navmesh_dirty = self._move_one_object( + new_obj, + navmesh_dirty, + translation=new_obj_translation, + rotation=new_obj_rotation, + ) + navmesh_dirty = new_navmesh_dirty or navmesh_dirty + res_objs.append(new_obj) + # duplicated all currently selected objects + # clear currently set selected objects + self._clear_sel_objs() + # Select all new objects + for new_obj in res_objs: + # add object to selected objects + self._add_obj_to_sel(new_obj) + return res_objs, navmesh_dirty + + else: + # No objects selected, get user input to load a single object + obj_substring = input( + "Load Object or AO. Enter a Rigid Object or AO handle substring, first match will be added:" + ).strip() + + if len(obj_substring) == 0: + print("No valid name given. Aborting") + return [], navmesh_dirty + return self.load_from_substring( + navmesh_dirty=navmesh_dirty, + obj_substring=obj_substring, + build_loc=build_loc, + ) + + def change_edit_mode(self, toggle: bool): + # toggle edit mode + mod_val = -1 if toggle else 1 + self.curr_edit_mode = ObjectEditor.EditMode( + (self.curr_edit_mode.value + ObjectEditor.EditMode.NUM_VALS.value + mod_val) + % ObjectEditor.EditMode.NUM_VALS.value + ) + + def change_edit_vals(self, toggle: bool): + # cycle through edit dist/amount multiplier + mod_val = -1 if toggle else 1 + self.curr_edit_multiplier = ObjectEditor.DistanceMode( + ( + self.curr_edit_multiplier.value + + ObjectEditor.DistanceMode.NUM_VALS.value + + mod_val + ) + % ObjectEditor.DistanceMode.NUM_VALS.value + ) + # update the edit values + self.set_edit_vals() + + def change_draw_box_types(self, toggle: bool): + # Cycle through types of objects to display with highlight box + mod_val = -1 if toggle else 1 + self.obj_type_to_draw = ObjectEditor.ObjectTypeToDraw( + ( + self.obj_type_to_draw.value + + ObjectEditor.ObjectTypeToDraw.NUM_VALS.value + + mod_val + ) + % ObjectEditor.ObjectTypeToDraw.NUM_VALS.value + ) + + def _draw_coordinate_axes(self, loc: mn.Vector3, debug_line_render): + # draw global coordinate axis + debug_line_render.draw_transformed_line( + loc - mn.Vector3.x_axis(), loc + mn.Vector3.x_axis(), mn.Color4.red() + ) + debug_line_render.draw_transformed_line( + loc - mn.Vector3.y_axis(), loc + mn.Vector3.y_axis(), mn.Color4.green() + ) + debug_line_render.draw_transformed_line( + loc - mn.Vector3.z_axis(), loc + mn.Vector3.z_axis(), mn.Color4.blue() + ) + debug_line_render.draw_circle( + loc + mn.Vector3.x_axis() * 0.95, + radius=0.05, + color=mn.Color4.red(), + normal=mn.Vector3.x_axis(), + ) + debug_line_render.draw_circle( + loc + mn.Vector3.y_axis() * 0.95, + radius=0.05, + color=mn.Color4.green(), + normal=mn.Vector3.y_axis(), + ) + debug_line_render.draw_circle( + loc + mn.Vector3.z_axis() * 0.95, + radius=0.05, + color=mn.Color4.blue(), + normal=mn.Vector3.z_axis(), + ) + + def _draw_selected_obj(self, obj, debug_line_render, box_color): + """ + Draw a selection box around and axis frame at the origin of a single object + """ + aabb = get_ao_root_bb(obj) if obj.is_articulated else obj.collision_shape_aabb + debug_line_render.push_transform(obj.transformation) + debug_line_render.draw_box(aabb.min, aabb.max, box_color) + debug_line_render.pop_transform() + + def draw_selected_objects(self, debug_line_render): + if len(self.sel_objs) == 0: + return + obj_list = self.sel_objs + sel_obj = obj_list[-1] + if sel_obj.is_alive: + # Last object selected is target object + self._draw_selected_obj( + sel_obj, + debug_line_render=debug_line_render, + box_color=mn.Color4.yellow(), + ) + self._draw_coordinate_axes( + sel_obj.translation, debug_line_render=debug_line_render + ) + mag_color = mn.Color4.magenta() + # draw all but last/target object + for i in range(len(obj_list) - 1): + obj = obj_list[i] + if obj.is_alive: + self._draw_selected_obj( + obj, debug_line_render=debug_line_render, box_color=mag_color + ) + self._draw_coordinate_axes( + obj.translation, debug_line_render=debug_line_render + ) + + def draw_box_around_objs(self, debug_line_render, agent_name: str = "hab_spot"): + """ + Draw a box of an object-type-specific color around every object in the scene (green for AOs and cyan for Rigids) + """ + if self.obj_type_to_draw.value % 2 == 1: + # draw aos if 1 or 3 + attr_mgr = self.sim.get_articulated_object_manager() + # Get all aos excluding the agent if present + new_sel_objs_dict = attr_mgr.get_objects_by_handle_substring( + search_str=agent_name, contains=False + ) + obj_clr = mn.Color4.green() + for obj in new_sel_objs_dict.values(): + if obj.is_alive: + self._draw_selected_obj( + obj, debug_line_render=debug_line_render, box_color=obj_clr + ) + + if self.obj_type_to_draw.value > 1: + # draw rigis if 2 or 3 + attr_mgr = self.sim.get_rigid_object_manager() + new_sel_objs_dict = attr_mgr.get_objects_by_handle_substring(search_str="") + obj_clr = mn.Color4.cyan() + for obj in new_sel_objs_dict.values(): + if obj.is_alive: + self._draw_selected_obj( + obj, debug_line_render=debug_line_render, box_color=obj_clr + ) diff --git a/src_python/habitat_sim/utils/classes/semantic_display.py b/src_python/habitat_sim/utils/classes/semantic_display.py new file mode 100644 index 0000000000..27e0db94f3 --- /dev/null +++ b/src_python/habitat_sim/utils/classes/semantic_display.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + + +from typing import Any, Dict + +import magnum as mn +import numpy as np + +import habitat_sim + + +# Class to display semantic settings in a scene +class SemanticDisplay: + def __init__(self, sim: habitat_sim.simulator.Simulator): + self.sim = sim + # Descriptive strings for semantic region debug draw possible choices + self.semantic_region_debug_draw_choices = ["None", "Kitchen Only", "All"] + # draw semantic region debug visualizations if present : should be [0 : len(semantic_region_debug_draw_choices)-1] + self.semantic_region_debug_draw_state = 0 + # Colors to use for each region's semantic rendering. + self.debug_semantic_colors: Dict[str, mn.Color4] = {} + + def cycle_semantic_region_draw(self): + new_state_idx = (self.semantic_region_debug_draw_state + 1) % len( + self.semantic_region_debug_draw_choices + ) + info_str = f"Change Region Draw from {self.semantic_region_debug_draw_choices[self.semantic_region_debug_draw_state]} to {self.semantic_region_debug_draw_choices[new_state_idx]}" + + # Increment visualize semantic bboxes. Currently only regions supported + self.semantic_region_debug_draw_state = new_state_idx + return info_str + + def draw_region_debug(self, debug_line_render: Any) -> None: + """ + Draw the semantic region wireframes. + """ + if self.semantic_region_debug_draw_state == 0: + return + if len(self.debug_semantic_colors) != len(self.sim.semantic_scene.regions): + self.debug_semantic_colors = {} + for region in self.sim.semantic_scene.regions: + self.debug_semantic_colors[region.id] = mn.Color4( + mn.Vector3(np.random.random(3)) + ) + if self.semantic_region_debug_draw_state == 1: + for region in self.sim.semantic_scene.regions: + if "kitchen" not in region.id.lower(): + continue + color = self.debug_semantic_colors.get(region.id, mn.Color4.magenta()) + for edge in region.volume_edges: + debug_line_render.draw_transformed_line( + edge[0], + edge[1], + color, + ) + else: + # Draw all + for region in self.sim.semantic_scene.regions: + color = self.debug_semantic_colors.get(region.id, mn.Color4.magenta()) + for edge in region.volume_edges: + debug_line_render.draw_transformed_line( + edge[0], + edge[1], + color, + ) diff --git a/src_python/habitat_sim/utils/common/__init__.py b/src_python/habitat_sim/utils/common/__init__.py new file mode 100755 index 0000000000..e40b207297 --- /dev/null +++ b/src_python/habitat_sim/utils/common/__init__.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +# List functions in __all__ to make available from this namespace level + + +from habitat_sim._ext.habitat_sim_bindings.core import orthonormalize_rotation_shear +from habitat_sim.utils.common.common import d3_40_colors_hex, d3_40_colors_rgb +from habitat_sim.utils.common.quaternion_utils import ( + angle_between_quats, + quat_from_angle_axis, + quat_from_coeffs, + quat_from_magnum, + quat_from_two_vectors, + quat_rotate_vector, + quat_to_angle_axis, + quat_to_coeffs, + quat_to_magnum, + random_quaternion, +) + +__all__ = [ + "angle_between_quats", + "orthonormalize_rotation_shear", + "quat_from_coeffs", + "quat_to_coeffs", + "quat_from_magnum", + "quat_to_magnum", + "quat_from_angle_axis", + "quat_to_angle_axis", + "quat_rotate_vector", + "quat_from_two_vectors", + "random_quaternion", + "d3_40_colors_hex", + "d3_40_colors_rgb", +] diff --git a/src_python/habitat_sim/utils/common/common.py b/src_python/habitat_sim/utils/common/common.py new file mode 100755 index 0000000000..a031b148d7 --- /dev/null +++ b/src_python/habitat_sim/utils/common/common.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +from typing import List + +import numpy as np + + +def colorize_ids(ids): + out = np.zeros((ids.shape[0], ids.shape[1], 3), dtype=np.uint8) + for i in range(ids.shape[0]): + for j in range(ids.shape[1]): + object_index = ids[i, j] + if object_index >= 0: + out[i, j] = d3_40_colors_rgb[object_index % 40] + return out + + +d3_40_colors_rgb: np.ndarray = np.array( + [ + [31, 119, 180], + [174, 199, 232], + [255, 127, 14], + [255, 187, 120], + [44, 160, 44], + [152, 223, 138], + [214, 39, 40], + [255, 152, 150], + [148, 103, 189], + [197, 176, 213], + [140, 86, 75], + [196, 156, 148], + [227, 119, 194], + [247, 182, 210], + [127, 127, 127], + [199, 199, 199], + [188, 189, 34], + [219, 219, 141], + [23, 190, 207], + [158, 218, 229], + [57, 59, 121], + [82, 84, 163], + [107, 110, 207], + [156, 158, 222], + [99, 121, 57], + [140, 162, 82], + [181, 207, 107], + [206, 219, 156], + [140, 109, 49], + [189, 158, 57], + [231, 186, 82], + [231, 203, 148], + [132, 60, 57], + [173, 73, 74], + [214, 97, 107], + [231, 150, 156], + [123, 65, 115], + [165, 81, 148], + [206, 109, 189], + [222, 158, 214], + ], + dtype=np.uint8, +) + + +# [d3_40_colors_hex] +d3_40_colors_hex: List[str] = [ + "0x1f77b4", + "0xaec7e8", + "0xff7f0e", + "0xffbb78", + "0x2ca02c", + "0x98df8a", + "0xd62728", + "0xff9896", + "0x9467bd", + "0xc5b0d5", + "0x8c564b", + "0xc49c94", + "0xe377c2", + "0xf7b6d2", + "0x7f7f7f", + "0xc7c7c7", + "0xbcbd22", + "0xdbdb8d", + "0x17becf", + "0x9edae5", + "0x393b79", + "0x5254a3", + "0x6b6ecf", + "0x9c9ede", + "0x637939", + "0x8ca252", + "0xb5cf6b", + "0xcedb9c", + "0x8c6d31", + "0xbd9e39", + "0xe7ba52", + "0xe7cb94", + "0x843c39", + "0xad494a", + "0xd6616b", + "0xe7969c", + "0x7b4173", + "0xa55194", + "0xce6dbd", + "0xde9ed6", +] +# [/d3_40_colors_hex] diff --git a/src_python/habitat_sim/utils/common.py b/src_python/habitat_sim/utils/common/quaternion_utils.py old mode 100755 new mode 100644 similarity index 64% rename from src_python/habitat_sim/utils/common.py rename to src_python/habitat_sim/utils/common/quaternion_utils.py index f6f309a0f5..c547d35c4c --- a/src_python/habitat_sim/utils/common.py +++ b/src_python/habitat_sim/utils/common/quaternion_utils.py @@ -5,19 +5,12 @@ # LICENSE file in the root directory of this source tree. import math -from io import BytesIO -from typing import List, Sequence, Tuple, Union -from urllib.request import urlopen -from zipfile import ZipFile +from typing import Sequence, Tuple, Union import magnum as mn import numpy as np import quaternion as qt -from habitat_sim._ext.habitat_sim_bindings.core import ( # noqa: F401 - orthonormalize_rotation_shear, -) - def quat_from_coeffs(coeffs: Union[Sequence[float], np.ndarray]) -> qt.quaternion: r"""Creates a quaternion from the coeffs returned by the simulator backend @@ -166,112 +159,3 @@ def random_quaternion(): ] ) return mn.Quaternion(qAxis, math.sqrt(1 - u[0]) * math.sin(2 * math.pi * u[1])) - - -def download_and_unzip(file_url, local_directory): - response = urlopen(file_url) - zipfile = ZipFile(BytesIO(response.read())) - zipfile.extractall(path=local_directory) - - -def colorize_ids(ids): - out = np.zeros((ids.shape[0], ids.shape[1], 3), dtype=np.uint8) - for i in range(ids.shape[0]): - for j in range(ids.shape[1]): - object_index = ids[i, j] - if object_index >= 0: - out[i, j] = d3_40_colors_rgb[object_index % 40] - return out - - -d3_40_colors_rgb: np.ndarray = np.array( - [ - [31, 119, 180], - [174, 199, 232], - [255, 127, 14], - [255, 187, 120], - [44, 160, 44], - [152, 223, 138], - [214, 39, 40], - [255, 152, 150], - [148, 103, 189], - [197, 176, 213], - [140, 86, 75], - [196, 156, 148], - [227, 119, 194], - [247, 182, 210], - [127, 127, 127], - [199, 199, 199], - [188, 189, 34], - [219, 219, 141], - [23, 190, 207], - [158, 218, 229], - [57, 59, 121], - [82, 84, 163], - [107, 110, 207], - [156, 158, 222], - [99, 121, 57], - [140, 162, 82], - [181, 207, 107], - [206, 219, 156], - [140, 109, 49], - [189, 158, 57], - [231, 186, 82], - [231, 203, 148], - [132, 60, 57], - [173, 73, 74], - [214, 97, 107], - [231, 150, 156], - [123, 65, 115], - [165, 81, 148], - [206, 109, 189], - [222, 158, 214], - ], - dtype=np.uint8, -) - - -# [d3_40_colors_hex] -d3_40_colors_hex: List[str] = [ - "0x1f77b4", - "0xaec7e8", - "0xff7f0e", - "0xffbb78", - "0x2ca02c", - "0x98df8a", - "0xd62728", - "0xff9896", - "0x9467bd", - "0xc5b0d5", - "0x8c564b", - "0xc49c94", - "0xe377c2", - "0xf7b6d2", - "0x7f7f7f", - "0xc7c7c7", - "0xbcbd22", - "0xdbdb8d", - "0x17becf", - "0x9edae5", - "0x393b79", - "0x5254a3", - "0x6b6ecf", - "0x9c9ede", - "0x637939", - "0x8ca252", - "0xb5cf6b", - "0xcedb9c", - "0x8c6d31", - "0xbd9e39", - "0xe7ba52", - "0xe7cb94", - "0x843c39", - "0xad494a", - "0xd6616b", - "0xe7969c", - "0x7b4173", - "0xa55194", - "0xce6dbd", - "0xde9ed6", -] -# [/d3_40_colors_hex] diff --git a/src_python/habitat_sim/utils/namespace/__init__.py b/src_python/habitat_sim/utils/namespace/__init__.py new file mode 100755 index 0000000000..0f0db8cad5 --- /dev/null +++ b/src_python/habitat_sim/utils/namespace/__init__.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. diff --git a/src_python/habitat_sim/utils/namespace/hsim_physics.py b/src_python/habitat_sim/utils/namespace/hsim_physics.py new file mode 100644 index 0000000000..c9b248c483 --- /dev/null +++ b/src_python/habitat_sim/utils/namespace/hsim_physics.py @@ -0,0 +1,405 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + + +from typing import Dict, List, Optional, Union + +import magnum as mn + +import habitat_sim +from habitat_sim import physics as HSim_Phys + + +def get_link_normalized_joint_position( + object_a: HSim_Phys.ManagedArticulatedObject, link_ix: int +) -> float: + """ + Normalize the joint limit range [min, max] -> [0,1] and return the current joint state in this range. + + :param object_a: The parent ArticulatedObject of the link. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + + :return: normalized joint position [0,1] + """ + + assert object_a.get_link_joint_type(link_ix) in [ + HSim_Phys.JointType.Revolute, + HSim_Phys.JointType.Prismatic, + ], f"Invalid joint type '{object_a.get_link_joint_type(link_ix)}'. Open/closed not a valid check for multi-dimensional or fixed joints." + + joint_pos_ix = object_a.get_link_joint_pos_offset(link_ix) + joint_pos = object_a.joint_positions[joint_pos_ix] + limits = object_a.joint_position_limits + + # compute the normalized position [0,1] + n_pos = (joint_pos - limits[0][joint_pos_ix]) / ( + limits[1][joint_pos_ix] - limits[0][joint_pos_ix] + ) + return n_pos + + +def set_link_normalized_joint_position( + object_a: HSim_Phys.ManagedArticulatedObject, + link_ix: int, + normalized_pos: float, +) -> None: + """ + Set the joint's state within its limits from a normalized range [0,1] -> [min, max] + + Assumes the joint has valid joint limits. + + :param object_a: The parent ArticulatedObject of the link. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + :param normalized_pos: The normalized position [0,1] to set. + """ + + assert object_a.get_link_joint_type(link_ix) in [ + HSim_Phys.JointType.Revolute, + HSim_Phys.JointType.Prismatic, + ], f"Invalid joint type '{object_a.get_link_joint_type(link_ix)}'. Open/closed not a valid check for multi-dimensional or fixed joints." + + assert ( + normalized_pos <= 1.0 and normalized_pos >= 0 + ), "values outside the range [0,1] are by definition beyond the joint limits." + + joint_pos_ix = object_a.get_link_joint_pos_offset(link_ix) + limits = object_a.joint_position_limits + joint_positions = object_a.joint_positions + joint_positions[joint_pos_ix] = limits[0][joint_pos_ix] + ( + normalized_pos * (limits[1][joint_pos_ix] - limits[0][joint_pos_ix]) + ) + object_a.joint_positions = joint_positions + + +def link_is_open( + object_a: HSim_Phys.ManagedArticulatedObject, + link_ix: int, + threshold: float = 0.4, +) -> bool: + """ + Check whether a particular AO link is in the "open" state. + We assume that joint limits define the closed state (min) and open state (max). + + :param object_a: The parent ArticulatedObject of the link to check. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + :param threshold: The normalized threshold ratio of joint ranges which are considered "open". E.g. 0.8 = 80% + + :return: Whether or not the link is considered "open". + """ + + return get_link_normalized_joint_position(object_a, link_ix) >= threshold + + +def link_is_closed( + object_a: HSim_Phys.ManagedArticulatedObject, + link_ix: int, + threshold: float = 0.1, +) -> bool: + """ + Check whether a particular AO link is in the "closed" state. + We assume that joint limits define the closed state (min) and open state (max). + + :param object_a: The parent ArticulatedObject of the link to check. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + :param threshold: The normalized threshold ratio of joint ranges which are considered "closed". E.g. 0.1 = 10% + + :return: Whether or not the link is considered "closed". + """ + + return get_link_normalized_joint_position(object_a, link_ix) <= threshold + + +def open_link(object_a: HSim_Phys.ManagedArticulatedObject, link_ix: int) -> None: + """ + Set a link to the "open" state. Sets the joint position to the maximum joint limit. + + TODO: does not do any collision checking to validate the state or move any other objects which may be contained in or supported by this link. + + :param object_a: The parent ArticulatedObject of the link to check. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + """ + + set_link_normalized_joint_position(object_a, link_ix, 1.0) + + +def close_link(object_a: HSim_Phys.ManagedArticulatedObject, link_ix: int) -> None: + """ + Set a link to the "closed" state. Sets the joint position to the minimum joint limit. + + TODO: does not do any collision checking to validate the state or move any other objects which may be contained in or supported by this link. + + :param object_a: The parent ArticulatedObject of the link to check. + :param link_ix: The index of the link within the parent object. Not the link's object_id. + """ + + set_link_normalized_joint_position(object_a, link_ix, 0) + + +def get_bb_corners(range3d: mn.Range3D) -> List[mn.Vector3]: + """ + Return a list of AABB (Range3D) corners in object local space. + """ + return [ + range3d.back_bottom_left, + range3d.back_bottom_right, + range3d.back_top_right, + range3d.back_top_left, + range3d.front_top_left, + range3d.front_top_right, + range3d.front_bottom_right, + range3d.front_bottom_left, + ] + + +def get_ao_root_bb( + ao: HSim_Phys.ManagedArticulatedObject, +) -> mn.Range3D: + """ + Get the local bounding box of all links of an articulated object in the root frame. + + :param ao: The ArticulatedObject instance. + """ + + # NOTE: we'd like to use SceneNode AABB, but this won't work because the links are not in the subtree of the root: + # ao.root_scene_node.compute_cumulative_bb() + + ao_local_part_bb_corners = [] + + link_nodes = [ao.get_link_scene_node(ix) for ix in range(-1, ao.num_links)] + for link_node in link_nodes: + local_bb_corners = get_bb_corners(link_node.cumulative_bb) + global_bb_corners = [ + link_node.absolute_transformation().transform_point(bb_corner) + for bb_corner in local_bb_corners + ] + ao_local_bb_corners = [ + ao.transformation.inverted().transform_point(p) for p in global_bb_corners + ] + ao_local_part_bb_corners.extend(ao_local_bb_corners) + + # get min and max of each dimension + # TODO: use numpy arrays for more elegance... + max_vec = mn.Vector3(ao_local_part_bb_corners[0]) + min_vec = mn.Vector3(ao_local_part_bb_corners[0]) + for point in ao_local_part_bb_corners: + for dim in range(3): + max_vec[dim] = max(max_vec[dim], point[dim]) + min_vec[dim] = min(min_vec[dim], point[dim]) + return mn.Range3D(min_vec, max_vec) + + +def get_ao_default_link( + ao: habitat_sim.physics.ManagedArticulatedObject, + compute_if_not_found: bool = False, +) -> Optional[int]: + """ + Get the "default" link index for a ManagedArticulatedObject. + The "default" link is the one link which should be used if only one joint can be actuated. For example, the largest or most accessible drawer or door. + + :param ao: The ManagedArticulatedObject instance. + :param compute_if_not_found: If true, try to compute the default link if it isn't found. + :return: The default link index or None if not found. Cannot be base link (-1). + + The default link is determined by: + + - must be "prismatic" or "revolute" joint type + - first look in the metadata Configuration for an annotated link. + - (if compute_if_not_found) - if not annotated, it is programmatically computed from a heuristic. + + Default link heuristic: the link with the lowest Y value in the bounding box with appropriate joint type. + """ + + # first look in metadata + default_link = ao.user_attributes.get("default_link") + + if default_link is None and compute_if_not_found: + valid_joint_types = [ + habitat_sim.physics.JointType.Revolute, + habitat_sim.physics.JointType.Prismatic, + ] + lowest_link = None + lowest_y: int = None + # compute the default link + for link_id in ao.get_link_ids(): + if ao.get_link_joint_type(link_id) in valid_joint_types: + # use minimum global keypoint Y value + link_lowest_y = min( + get_articulated_link_global_keypoints(ao, link_id), + key=lambda x: x[1], + )[1] + if lowest_y is None or link_lowest_y < lowest_y: + lowest_y = link_lowest_y + lowest_link = link_id + if lowest_link is not None: + default_link = lowest_link + # if found, set in metadata for next time + ao.user_attributes.set("default_link", default_link) + + return default_link + + +def get_ao_link_id_map(sim: habitat_sim.Simulator) -> Dict[int, int]: + """ + Construct a dict mapping ArticulatedLink object_id to parent ArticulatedObject object_id. + NOTE: also maps ao's root object id to itself for ease of use. + + :param sim: The Simulator instance. + + :return: dict mapping ArticulatedLink object ids to parent object ids. + """ + + aom = 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 + + return ao_link_map + + +def get_global_keypoints_from_bb( + aabb: mn.Range3D, local_to_global: mn.Matrix4 +) -> List[mn.Vector3]: + """ + Get a list of bounding box keypoints in global space. + 0th point is the bounding box center, others are bounding box corners. + + :param aabb: The local bounding box. + :param local_to_global: The local to global transformation matrix. + + :return: A set of global 3D keypoints for the bounding box. + """ + local_keypoints = [aabb.center()] + local_keypoints.extend(get_bb_corners(aabb)) + global_keypoints = [ + local_to_global.transform_point(key_point) for key_point in local_keypoints + ] + return global_keypoints + + +def get_articulated_link_global_keypoints( + object_a: habitat_sim.physics.ManagedArticulatedObject, link_index: int +) -> List[mn.Vector3]: + """ + Get global bb keypoints for an ArticulatedLink. + + :param object_a: The parent ManagedArticulatedObject for the link. + :param link_index: The local index of the link within the parent ArticulatedObject. Not the object_id of the link. + + :return: A set of global 3D keypoints for the link. + """ + link_node = object_a.get_link_scene_node(link_index) + + return get_global_keypoints_from_bb( + link_node.cumulative_bb, link_node.absolute_transformation() + ) + + +def get_obj_from_id( + sim: habitat_sim.Simulator, + obj_id: int, + ao_link_map: Optional[Dict[int, int]] = None, +) -> Union[HSim_Phys.ManagedRigidObject, HSim_Phys.ManagedArticulatedObject,]: + """ + Get a ManagedRigidObject or ManagedArticulatedObject from an object_id. + + ArticulatedLink object_ids will return the ManagedArticulatedObject. + If you want link id, use ManagedArticulatedObject.link_object_ids[obj_id]. + + :param sim: The Simulator instance. + :param obj_id: object id for which ManagedObject is desired. + :param ao_link_map: A pre-computed map from link object ids to their parent ArticulatedObject's object id. + + :return: a ManagedObject or None + """ + + if ao_link_map is None: + # Note: better to pre-compute this and pass it around + ao_link_map = get_ao_link_id_map(sim) + + aom = sim.get_articulated_object_manager() + if obj_id in ao_link_map: + return aom.get_object_by_id(ao_link_map[obj_id]) + + rom = sim.get_rigid_object_manager() + if rom.get_library_has_id(obj_id): + return rom.get_object_by_id(obj_id) + + return None + + +def get_obj_from_handle( + sim: habitat_sim.Simulator, obj_handle: str +) -> Union[HSim_Phys.ManagedRigidObject, HSim_Phys.ManagedArticulatedObject,]: + """ + Get a ManagedRigidObject or ManagedArticulatedObject from its instance handle. + + :param sim: The Simulator instance. + :param obj_handle: object instance handle for which ManagedObject is desired. + + :return: a ManagedObject or None + """ + aom = sim.get_articulated_object_manager() + if aom.get_library_has_handle(obj_handle): + return aom.get_object_by_handle(obj_handle) + + rom = sim.get_rigid_object_manager() + if rom.get_library_has_handle(obj_handle): + return rom.get_object_by_handle(obj_handle) + + return None + + +def get_all_ao_objects( + sim: habitat_sim.Simulator, +) -> List[HSim_Phys.ManagedArticulatedObject]: + """ + Get a list of all ManagedArticulatedObjects in the scene. + + :param sim: The Simulator instance. + + :return: a list of ManagedObject wrapper instances containing all articulated objects currently instantiated in the scene. + """ + return ( + sim.get_articulated_object_manager().get_objects_by_handle_substring().values() + ) + + +def get_all_rigid_objects( + sim: habitat_sim.Simulator, +) -> List[HSim_Phys.ManagedArticulatedObject]: + """ + Get a list of all ManagedRigidObjects in the scene. + + :param sim: The Simulator instance. + + :return: a list of ManagedObject wrapper instances containing all rigid objects currently instantiated in the scene. + """ + return sim.get_rigid_object_manager().get_objects_by_handle_substring().values() + + +def get_all_objects( + sim: habitat_sim.Simulator, +) -> List[Union[HSim_Phys.ManagedRigidObject, HSim_Phys.ManagedArticulatedObject,]]: + """ + Get a list of all ManagedRigidObjects and ManagedArticulatedObjects in the scene. + + :param sim: The Simulator instance. + + :return: a list of ManagedObject wrapper instances containing all objects currently instantiated in the scene. + """ + + managers = [ + sim.get_rigid_object_manager(), + sim.get_articulated_object_manager(), + ] + all_objects = [] + for mngr in managers: + all_objects.extend(mngr.get_objects_by_handle_substring().values()) + return all_objects diff --git a/src_python/habitat_sim/utils/sim_utils.py b/src_python/habitat_sim/utils/sim_utils.py new file mode 100644 index 0000000000..5d6882589d --- /dev/null +++ b/src_python/habitat_sim/utils/sim_utils.py @@ -0,0 +1,424 @@ +#!/usr/bin/env python3 + +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + + +import os +from typing import Callable, List + +import magnum as mn +import numpy as np + +import habitat_sim +from habitat_sim import physics as HSim_Phys + + +# Class to instantiate and maneuver spot from a viewer +# DEPENDENT ON HABITAT-LAB - +class SpotAgent: + from habitat.articulated_agents.robots.spot_robot import SpotRobot + from habitat.datasets.rearrange.navmesh_utils import get_largest_island_index + from omegaconf import DictConfig + + 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" + + class ExtractedBaseVelNonCylinderAction: + def __init__(self, sim, spot): + self._sim = sim + self.spot = spot + self.base_vel_ctrl = HSim_Phys.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 + self._noclip = False + # If we just changed from noclip to clip - make sure + # that if spot is not on the navmesh he gets snapped to it + self._transition_to_clip = False + + 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 + """ + + def step_spot( + num_check_cylinder: int, + cur_height: float, + pos_calc: Callable[[np.ndarray, np.ndarray], np.ndarray], + cur_pos: List[np.ndarray], + goal_pos: List[np.ndarray], + ): + end_pos = [] + for i in range(num_check_cylinder): + pos = pos_calc(cur_pos[i], goal_pos[i]) + # Sanitize the height + pos[1] = cur_height + cur_pos[i][1] = cur_height + goal_pos[i][1] = cur_height + end_pos.append(pos) + return end_pos + + # 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: List[np.ndarray] = [ + trans.transform_point(xyz) for xyz in nav_pos_3d + ] + goal_pos: List[np.ndarray] = [ + target_trans.transform_point(xyz) for xyz in nav_pos_3d + ] + + # For step filter of offset positions + end_pos = [] + + no_filter_step = lambda _, val: val + if self._noclip: + cur_height = self.spot.base_pos[1] + # ignore navmesh + end_pos = step_spot( + num_check_cylinder=num_check_cylinder, + cur_height=cur_height, + pos_calc=no_filter_step, + cur_pos=cur_pos, + goal_pos=goal_pos, + ) + else: + cur_height = self._sim.pathfinder.snap_point(self.spot.base_pos)[1] + # constrain to navmesh + end_pos = step_spot( + num_check_cylinder=num_check_cylinder, + cur_height=cur_height, + pos_calc=self._sim.step_filter, + cur_pos=cur_pos, + goal_pos=goal_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 + inv_ctrl_freq = 1.0 / 60.0 + # 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( + inv_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 toggle_clip(self, largest_island_ix: int): + """ + Handle transition to/from no clipping/navmesh disengaged. + """ + # Transitioning to clip from no clip or vice versa + self._transition_to_clip = self._noclip + self._noclip = not self._noclip + + spot_cur_point = self.spot.base_pos + # Find reasonable location to return to navmesh + if self._transition_to_clip and not self._sim.pathfinder.is_navigable( + spot_cur_point + ): + # Clear transition flag - only transition once + self._transition_to_clip = False + # Find closest point on navmesh to snap spot to + print( + f"Trying to find closest navmesh point to spot_cur_point: {spot_cur_point}" + ) + new_point = self._sim.pathfinder.snap_point( + spot_cur_point, largest_island_ix + ) + if not np.any(np.isnan(new_point)): + print( + f"Closest navmesh point to spot_cur_point: {spot_cur_point} is {new_point} on largest island {largest_island_ix}. Snapping to it." + ) + # Move spot to this point + self.spot.base_pos = new_point + else: + # try again to any island + new_point = self._sim.pathfinder.snap_point(spot_cur_point) + if not np.any(np.isnan(new_point)): + print( + f"Closest navmesh point to spot_cur_point: {spot_cur_point} is {new_point} not on largest island. Snapping to it." + ) + # Move spot to this point + self.spot.base_pos = new_point + else: + print( + "Unable to leave no-clip mode, too far from navmesh. Try again when closer." + ) + self._noclip = True + return self._noclip + + 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) + + def __init__(self, sim: habitat_sim.Simulator): + self.sim = sim + # changed when spot is put on navmesh + self.largest_island_ix = -1 + self.spot_forward = 0.0 + self.spot_lateral = 0.0 + self.spot_angular = 0.0 + self.load_and_init() + # angle and azimuth of camera orientation + self.camera_angles = mn.Vector2() + self.init_spot_cam() + + self.spot_rigid_state = self.spot.sim_obj.rigid_state + self.spot_motion_type = self.spot.sim_obj.motion_type + + def load_and_init(self): + # add the robot to the world via the wrapper + robot_path = SpotAgent.SPOT_DIR + agent_config = SpotAgent.DictConfig({"articulated_agent_urdf": robot_path}) + self.spot: SpotAgent.SpotRobot = SpotAgent.SpotRobot( + agent_config, self.sim, fixed_base=True + ) + self.spot.reconfigure() + self.spot.update() + self.spot_action: SpotAgent.ExtractedBaseVelNonCylinderAction = ( + SpotAgent.ExtractedBaseVelNonCylinderAction(self.sim, self.spot) + ) + + def init_spot_cam(self): + # Camera relative to spot + self.camera_distance = 2.0 + # height above spot to target lookat + self.lookat_height = 0.0 + + def mod_spot_cam( + self, + scroll_mod_val: float = 0, + mse_rel_pos: List = None, + shift_pressed: bool = False, + alt_pressed: bool = False, + ): + """ + Modify the camera agent's orientation, distance and lookat target relative to spot via UI input + """ + # use shift for fine-grained zooming + if scroll_mod_val != 0: + mod_val = 0.3 if shift_pressed else 0.15 + scroll_delta = scroll_mod_val * mod_val + if alt_pressed: + # lookat going up and down + self.lookat_height -= scroll_delta + else: + self.camera_distance -= scroll_delta + if mse_rel_pos is not None: + self.camera_angles[0] -= mse_rel_pos[1] * 0.01 + self.camera_angles[1] -= mse_rel_pos[0] * 0.01 + self.camera_angles[0] = max(-1.55, min(0.5, self.camera_angles[0])) + self.camera_angles[1] = np.fmod(self.camera_angles[1], np.pi * 2.0) + + def set_agent_camera_transform(self, agent_node): + # set camera 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) + ) + spot_pos = self.base_pos() + lookat_disp = mn.Vector3(0, self.lookat_height, 0) + lookat_pos = spot_pos + lookat_disp + camera_position = local_camera_position + lookat_pos + agent_node.transformation = mn.Matrix4.look_at( + camera_position, + lookat_pos, + mn.Vector3(0, 1, 0), + ) + + def base_pos(self): + return self.spot.base_pos + + def place_on_navmesh(self): + if self.sim.pathfinder.is_loaded: + self.largest_island_ix = SpotAgent.get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) + print(f"Largest indoor island index = {self.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( + island_index=self.largest_island_ix + ) + 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: + self.spot.base_pos = valid_spot_point + else: + print( + f"Unable to find a valid spot for Spot on the navmesh after {max_attempts} attempts" + ) + + def toggle_clip(self): + # attempt to turn on or off noclip + clipstate = self.spot_action.toggle_clip(self.largest_island_ix) + + # Turn off dynamics if spot is being moved kinematically + # self.spot.sim_obj.motion_type = HSim_MT.KINEMATIC if clipstate else self.spot_motion_type + print(f"After toggle, clipstate is {clipstate}") + + def move_spot( + self, + move_fwd: bool, + move_back: bool, + move_up: bool, + move_down: bool, + slide_left: bool, + slide_right: bool, + turn_left: bool, + turn_right: bool, + ): + inc = 0.02 + min_val = 0.1 + + if move_fwd and not move_back: + self.spot_forward = max(min_val, self.spot_forward + inc) + elif move_back and not move_fwd: + self.spot_forward = min(-min_val, self.spot_forward - inc) + else: + self.spot_forward *= 0.5 + if abs(self.spot_forward) < min_val: + self.spot_forward = 0 + + if slide_left and not slide_right: + self.spot_lateral = max(min_val, self.spot_lateral + inc) + elif slide_right and not slide_left: + self.spot_lateral = min(-min_val, self.spot_lateral - inc) + else: + self.spot_lateral *= 0.5 + if abs(self.spot_lateral) < min_val: + self.spot_lateral = 0 + + if turn_left and not turn_right: + self.spot_angular = max(min_val, self.spot_angular + inc) + elif turn_right and not turn_left: + self.spot_angular = min(-min_val, self.spot_angular - inc) + else: + self.spot_angular *= 0.5 + 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 cache_transform_and_remove(self): + """ + Save spot's current location and remove from scene, for saving scene instance. + """ + aom = self.sim.get_articulated_object_manager() + self.spot_rigid_state = self.spot.sim_obj.rigid_state + aom.remove_object_by_id(self.spot.sim_obj.object_id) + + def restore_at_previous_loc(self): + """ + Reload spot and restore from saved location. + """ + # rebuild spot + self.load_and_init() + # put em back + self.spot.sim_obj.rigid_state = self.spot_rigid_state + + def get_point_in_front(self, disp_in_front: mn.Vector3 = None): + if disp_in_front is None: + disp_in_front = [1.5, 0.0, 0.0] + return self.spot.base_transformation.transform_point(disp_in_front) diff --git a/tools/batched_armature_to_urdf.py b/tools/batched_armature_to_urdf.py new file mode 100644 index 0000000000..25a326c7c0 --- /dev/null +++ b/tools/batched_armature_to_urdf.py @@ -0,0 +1,239 @@ +# 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 collections import defaultdict +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(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 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) + base_command = f"blender {blend_file} --background --python {script_path} -- --export-path {export_path}" + # first export the meshes + os.system(base_command + " --export-meshes --fix-materials") + # then export the URDF + os.system( + base_command + + " --export-urdf --export-ao-config --round-collision-scales --fix-collision-scales" + ) + + +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)) + ] + + +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 +# 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, + ) + 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, + ) + 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.", + ) + 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 + 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)) + + if args.no_replace: + # 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 + 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)}") + 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 + 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)) + + 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, + export_path=args.out_dir, + script_path=args.converter_script_path, + ) + + +if __name__ == "__main__": + main() diff --git a/tools/blender_armature_to_urdf.py b/tools/blender_armature_to_urdf.py new file mode 100644 index 0000000000..1764c5d390 --- /dev/null +++ b/tools/blender_armature_to_urdf.py @@ -0,0 +1,1032 @@ +# 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 json +import math +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 Matrix, 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 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 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. + """ + 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 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_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_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 (is_rec_mesh and include_receptacle) + or (include_render and not is_col_mesh and not is_rec_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_render, + include_collison, + include_receptacle, + ) + ) + return selected_objects + + +def walk_armature(this_bone, handler, kwargs_for_handler=None): + """ + Recursively apply a handler function to bone children to traverse the armature. + """ + 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, kwargs_for_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, + 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""" + + 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 + + # 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 = [] + receptacle_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_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 + 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 link_visuals: + this_xml_link.append(this_xml_visual) + + # NOTE: visual debugging tool to add a box at the joint pivot locations + if joint_visuals: + 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() + + supported_collision_shapes = [ + "collision_box", + "collision_cylinder", + "collision_sphere", + ] + + for col in collision_objects: + assert ( + 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( + 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 + elif "collision_cylinder" in col.name: + this_xml_cyl = ET.Element("cylinder") + scale = col.scale + # radius XY axis scale must match + 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 + 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 + 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 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. + + 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) + + local_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) + 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}") + 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 + 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)) + + 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 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 ac.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 + 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 + ), 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 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, + export_urdf: bool = True, + export_meshes: bool = True, + export_ao_config: bool = True, + fix_materials: 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 = [] + 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") + + # get the armature + armature = settings.get("armature") + if armature is None: + 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"] + + 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: + 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 + receptacle_meshes = [] + receptacle_to_link_name = {} + 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}") + if is_receptacle_mesh(obj): + receptacle_meshes.append(obj) + 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: + 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) + print(f"Output path : {final_out_path}") + + # 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, + ) + # export receptacle meshes + for rec_mesh in receptacle_meshes: + clear_obj_transform(rec_mesh.parent) + deselect_all() + rec_mesh.select_set(True) + bpy.ops.export_scene.gltf( + filepath=os.path.join(final_out_path, rec_mesh.name), + use_selection=True, + export_yup=False, + ) + + # print("------------------------") + # print("Bone info recursion:") + # walk_armature(root_bone, bone_info) + # print("------------------------") + # 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) + + # 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) + + 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() + + output_path = os.path.join(final_out_path, f"{root_node.name}.urdf") + + 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 + ao_config_contents = { + "urdf_filepath": f"{root_node.name}.urdf", + "user_defined": { + # insert receptacle metadata here + }, + } + for rec_name, link_name in receptacle_to_link_name.items(): + 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, + "position": [0, 0, 0], + "rotation": [1, 0, 0, 0], + "scale": [1, 1, 1], + "up": [0, 0, 1], + "mesh_filepath": rec_name + ".glb", + } + 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: + json.dump(ao_config_contents, f) + + return output_path + + +if __name__ == "__main__": + # 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: + 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_urdf = False + export_meshes = False + export_ao_config = False + round_collision_scales = False + fix_collision_scales = False + fix_materials = False + + # visual shape export flags for debugging + link_visuals = True + collision_visuals = False + joint_visuals = False + receptacle_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 arguments, so we pass anything after that into the script arguments 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.", + ) + parser.add_argument( + "--export-ao-config", + action="store_true", + 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", + 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.", + ) + 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.", + ) + 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).", + ) + parser.add_argument( + "--fix-collision-scales", + action="store_true", + 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 + 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 ( + 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(), + "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 + }, + 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, + receptacle_visuals=args.receptacle_visuals, + ) + print(f"\n ======== Output saved to {output_path} ========\n") diff --git a/tools/check_siro_aos.py b/tools/check_siro_aos.py new file mode 100644 index 0000000000..bca4cfed0e --- /dev/null +++ b/tools/check_siro_aos.py @@ -0,0 +1,268 @@ +import os +from typing import Any, Dict, List + +# 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)] + + +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 + + +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 + + 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 + + mi = MetadataInterface(default_metadata_dict) + + # 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: + + ########################################### + # 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) + ao_test_results[ao_short_handle]["failure_log"] = to_str_csv( + asset_failure_message + ) + + ########################################### + # 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() + + # 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/check_siro_scenes.py b/tools/check_siro_scenes.py new file mode 100644 index 0000000000..10b6648472 --- /dev/null +++ b/tools/check_siro_scenes.py @@ -0,0 +1,1145 @@ +import json +import os +from collections import defaultdict +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 + +# NOTE: (requires habitat-llm) get metadata for semantics +# from dataset_generation.benchmark_generation.generate_episodes import ( +# MetadataInterface, +# default_metadata_dict, +# ) +from habitat.datasets.rearrange.navmesh_utils import ( + embodied_unoccluded_navmesh_snap, + 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 NavMeshSettings, Simulator +from habitat_sim.metadata import MetadataMediator +from habitat_sim.physics import MotionType +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 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 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"{to_str_csv(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 + + +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): + 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"{normalized_region_id}_" + ) + + +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}") + + +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 20 objects on the receptacle + target_number = 20 + 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( + pos=obj.translation, + height=1.3, + pathfinder=sim.pathfinder, + sim=sim, + target_object_ids=[obj.object_id], + island_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 + + # first cache AO motion types and set to STATIC for navmesh + ao_motion_types = [] + for ao in ( + sim.get_articulated_object_manager().get_objects_by_handle_substring().values() + ): + # ignore the robot + if "hab_spot" not in ao.handle: + ao_motion_types.append((ao, ao.motion_type)) + ao.motion_type = MotionType.STATIC + + sim.recompute_navmesh( + sim.pathfinder, + navmesh_settings, + ) + + # reset AO motion types from cache + for ao, ao_orig_motion_type in ao_motion_types: + ao.motion_type = ao_orig_motion_type + + +def read_split_yaml(split_yaml: str) -> Dict[str, List[str]]: + """ + Parses the split yaml file to get a dict of split -> scene ids. + """ + import yaml + + assert os.path.exists(split_yaml), f"split yaml: '{split_yaml}' does not exist." + # read yaml file + with open(split_yaml, "r") as f: + scene_splits = yaml.safe_load(f) + return scene_splits + + +def get_split(curr_scene_name, splits): + """ + Get the split of the current scene + """ + for label in splits: + if curr_scene_name in splits[label]: + return label + return "test" + + +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. + + :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() + + # 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) + + # dbv = DebugVisualizer(sim) + # breakpoint() + + # 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 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, + rec_filter_dict, + ignore_existing_status=ignore_existing_status, + ) + 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 obj.is_articulated] + for aoix, ao in enumerate(aos): + default_link = sutils.get_ao_default_link(ao, True) + if default_link is not None: + # print(f"found default_link = {default_link}") + 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" + ) + write_rec_filter_json(filter_filepath, rec_filter_dict) + + +def try_load_rec_filter(sim): + """ + Attempt to find and load the receptacle filter file configured for the current scene. + Return whether or not the filter file was successfully loaded. + """ + rec_filter_filepath = hab_receptacle.get_scene_rec_filter_filepath( + sim.metadata_mediator, sim.curr_scene_name + ) + if rec_filter_filepath is None: + return False + rec_filter_paths = hab_receptacle.get_excluded_recs_from_filter_file( + rec_filter_filepath + ) + if len(rec_filter_paths) > 0: + return True + return False + + +def draw_receptacles(sim, receptacles, selected_rec_unique_name: Optional[str] = None): + """Debug draw callback for dbv to render the Receptacles.""" + scene_filter_file = hab_receptacle.get_scene_rec_filter_filepath( + sim.metadata_mediator, sim.curr_scene_name + ) + filter_strings = hab_receptacle.get_excluded_recs_from_filter_file( + scene_filter_file + ) + for rec in receptacles: + color = mn.Color4.green() + if rec.unique_name == selected_rec_unique_name: + color = mn.Color4.cyan() + elif rec.unique_name in filter_strings: + color = mn.Color4.red() + rec.debug_draw(sim, color=color) + + +def flag_non_default_link_active_recs( + sim: Simulator, +) -> List[hab_receptacle.Receptacle]: + """ + Detects any Receptacles attached to moveable ArticulatedLinks which are not the "default link". + These Receptacles may be edge cases of the automated accessibility checks in "run_rec_filter_analysis" because, for example, they have open-fronted drawers. + :return: The list of Receptacles triggering this flag if any are found. + """ + # rec_filter_filepath = hab_receptacle.get_scene_rec_filter_filepath( + # sim.metadata_mediator, sim.curr_scene_name + # ) + # NOTE: redirected to output of previous process + rec_filter_filepath = ( + f"siro_test_results/scene_filter_files/{sim.curr_scene_name}.rec_filter.json" + ) + if rec_filter_filepath is None: + return [] + deactivated_rec_unique_names = hab_receptacle.get_excluded_recs_from_filter_file( + rec_filter_filepath + ) + non_default_active_recs = [] + recs = hab_receptacle.find_receptacles(sim) + for rec in recs: + if ( + rec.parent_link is not None + and rec.parent_link > 0 + and rec.unique_name not in deactivated_rec_unique_names + ): + # this is an active Receptacle on a non-body link + rec_parent = sutils.get_obj_from_handle(sim, rec.parent_object_handle) + ao_default_link = sutils.get_ao_default_link( + rec_parent, compute_if_not_found=True + ) + if ao_default_link != rec.parent_link: + print( + f"ao_default_link = {ao_default_link}, this link = {rec.parent_link}" + ) + non_default_active_recs.append(rec) + dbv = DebugVisualizer(sim) + relevant_recs = [ + _rec + for _rec in recs + if _rec.parent_object_handle == rec.parent_object_handle + ] + dbv.dblr_callback = draw_receptacles + dbv.dblr_callback_params = { + "sim": sim, + "receptacles": relevant_recs, + "selected_rec_unique_name": rec.unique_name, + } + # dbv.peek(rec_parent, peek_all_axis=True).show() + dbv.peek(rec_parent, peek_all_axis=True).save( + "siro_test_results/non_default_active_recs/", prefix=rec.unique_name + ) + # breakpoint() + dbv.remove_dbv_agent() + + return non_default_active_recs + + +def try_find_faucets(sim) -> Tuple[bool, int, int, int, int]: + """ + Try to get faucets on objects in the scene. + :return: boolean whether or not there are faucet annotations, number of faucet objects, number of faucet objects with receptacles, number of faucet objects with active receptacles, number of navigable faucet objs + """ + + # first find all faucet annotations + objs = sutils.get_all_objects(sim) + obj_markersets: Dict[str, List[mn.Vector3]] = {} + 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) + objs_w_faucets = obj_markersets.keys() + objs_w_faucets = list(set(objs_w_faucets)) + + if len(objs_w_faucets) == 0: + return False, 0, 0 + + navigable_faucet_objs = [] + if True: + largest_island_ix = get_largest_island_index( + pathfinder=sim.pathfinder, + sim=sim, + allow_outdoor=False, + ) + for obj_handle in objs_w_faucets: + is_navigable = try_nav_faucet_point(sim, obj_handle, largest_island_ix) + if is_navigable: + navigable_faucet_objs.append(obj_handle) + + # then find all receptacles + all_recs = hab_receptacle.find_receptacles(sim) + all_rec_objs = [rec.parent_object_handle for rec in all_recs] + all_rec_objs = list(set(all_rec_objs)) + + # also check the filtered recs + rec_filter_filepath = hab_receptacle.get_scene_rec_filter_filepath( + sim.metadata_mediator, sim.curr_scene_name + ) + rec_filter_paths = hab_receptacle.get_excluded_recs_from_filter_file( + rec_filter_filepath + ) + filtered_rec_objs = [ + rec.parent_object_handle + for rec in all_recs + if rec.unique_name not in rec_filter_paths + ] + filtered_rec_objs = list(set(filtered_rec_objs)) + + all_faucet_recs = [ + obj_handle for obj_handle in objs_w_faucets if obj_handle in all_rec_objs + ] + filtered_faucet_recs = [ + obj_handle for obj_handle in objs_w_faucets if obj_handle in filtered_rec_objs + ] + return ( + True, + len(objs_w_faucets), + len(all_faucet_recs), + len(filtered_faucet_recs), + len(navigable_faucet_objs), + ) + + +def try_nav_faucet_point(sim, faucet_obj_handle, largest_island_ix): + """ + Use nav utils to try finding a placement for the spot robot which can access a faucet + """ + robot_body_offsets = [[0.0, 0.0], [0.25, 0.0], [-0.25, 0.0]] + faucet_obj = sutils.get_obj_from_handle(sim, faucet_obj_handle) + + faucet_points = [] + all_obj_marker_sets = faucet_obj.marker_sets + if all_obj_marker_sets.has_taskset("faucets"): + # this object has faucet annotations + 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 = faucet_obj.get_link_id_from_name(link_name) + for _marker_subset_name, points in link_faucet_markers.items(): + global_points = faucet_obj.transform_local_pts_to_world(points, link_id) + faucet_points.extend(global_points) + if len(faucet_points) == 0: + return False + obj_ids = [faucet_obj.object_id] + if faucet_obj.is_articulated: + obj_ids.extend(list(faucet_obj.link_object_ids.keys())) + point_navigability = [] + for point in faucet_points: + nav_point, orientation, success = embodied_unoccluded_navmesh_snap( + target_position=point, + height=1.3, + sim=sim, + ignore_object_ids=obj_ids, + embodiment_heuristic_offsets=robot_body_offsets, + island_id=largest_island_ix, + ) + point_navigability.append(success) + return any(point_navigability) + + +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.", + ) + parser.add_argument( + "--actions", + nargs="+", + type=str, + help="A set of strings indicating check actions to be performed on the dataset.", + default=None, + ) + parser.add_argument( + "--scenes", + nargs="+", + type=str, + help="A subset of scene names to process. Limits the iteration to less than the full set of scenes.", + default=None, + ) + + args = parser.parse_args() + + available_check_actions = [ + "rec_unique_names", + "rec_filters", + "faucets", + "region_counts", + "joint_popping", + "visualize_regions", + "analyze_semantics", + "splits", + ] + + 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 + 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 + + # mi = MetadataInterface(default_metadata_dict) + + # 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) + + target_scenes = mm.get_scene_handles() + if args.scenes is not None: + target_scenes = args.scenes + + # Skip scenes + skip_scenes = [ + "102817119", + "102344469", + "102344328", + "108736779_1772634840", + "108294846_176710506", + "108294492_176709993", + "107734158_175999998", + "107734146_175999971", + "107734119_175999938", + "106879023_174887148", + "106878975_174887088", + "106878975_174887088", + "106366104_174226332", + "106366104_174226329", + "106365897_174225972", + "105515403_173104449", + "105515211_173104185", + "105515175_173104107", + "105515160_173104077", + "105515151_173104068", + "104348328_171513363", + "104348289_171513294", + "103997718_171030855", + "103997643_171030747", + "103997613_171030702", + "103997586_171030669", + "103997562_171030642", + "103997586_171030666", + "102816066", + "102343992", + "102816150", # this one has the broken ao instance in the scene instance + "104862396_172226349", # this one has broken ao instance + "104862573_172226682", # this one has the broken user_defined receptacles ao instance in the scene instance + # already done: + "102343992", + "102344094", + "102344115", + "102344307", + "102344328", + "102344349", + "102344439", + "108736800_177263517", + "108736884_177263634", + "EMPTY_TEST", + "102815859_169535055", + "102816036", + "102816114", + "102816051", + # next batch (09_25 PM) + "103997940_171031257", + "103997865_171031116", + "103997799_171031002", + "103997781_171030978", + "103997730_171030885", + "103997541_171030615", + "103997478_171030528", + "103997478_171030525", + "103997445_171030492", + "103997403_171030405", + "102817053 102816852", + "102816786 102816729", + "102816627 102816615", + "102816600", + "102816615", + "102816627", + "102816729", + "102816786", + "104862384_172226319", + "104862369_172226304", + "104862345_172226274", + "104348511_171513654", + "104348478_171513603", + "104348394_171513453", + "104348253_171513237", + "104348202_171513150", + "104348181_171513120", + "104348160_171513093", + "104348133_171513054", + "104348103_171513021", + "104348064_171512940", + "104348037_171512898", + "104348028_171512877", + "103997994_171031320", + "103997970_171031287", + "102817053", + "102816852", + "104862558_172226664", + "104862534_172226625", + "104862513_172226580", + "104862501_172226556", + "104862417_172226382", + ] + + new_target_scenes = [] + for scene_handle in target_scenes: + short_handle = scene_handle.split("/")[-1].split(".")[0] + if short_handle not in skip_scenes: + new_target_scenes.append(short_handle) + target_scenes = new_target_scenes + print(target_scenes) + # breakpoint() + + ########################################## + # get each scene's split + scene_splits = {} + split_file = args.dataset[: -len(args.dataset.split("/")[-1])] + "scene_splits.yaml" + splits = read_split_yaml(split_file) + for _s_ix, scene_handle in enumerate(target_scenes): + scene_name = scene_handle.split("/")[-1].split(".")[0] + scene_split = get_split(scene_name, splits) + scene_splits[scene_name] = scene_split + + # NOTE: hack to limit scenes to a particular split + # target_scenes = [scene_handle for scene_handle in target_scenes if scene_splits[scene_handle.split("/")[-1].split(".")[0]]=="test"] + num_scenes = len(target_scenes) + + # for each scene, initialize a fresh simulator and run tests + for s_ix, scene_handle in enumerate(target_scenes): + 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) + + # mi.refresh_scene_caches(sim) + + navmesh_config_and_recompute(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}/") + + # cache scene split metadata + if "splits" in target_check_actions: + scene_test_results[sim.curr_scene_name]["split"] = scene_splits[ + 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 "rec_filters" in target_check_actions: + # check for functional filter file + filter_working = try_load_rec_filter(sim) + scene_test_results[sim.curr_scene_name][ + "rec_filter_working" + ] = filter_working + + # run the accessibility check and produce a filter file + run_rec_filter_analysis( + sim, args.out_dir, open_default_links=True, keep_manual_filters=True + ) + + # check non-default active Receptacles + # non_default_active_recs = flag_non_default_link_active_recs(sim) + # if len(non_default_active_recs) > 0: + # scene_test_results[sim.curr_scene_name]["non_default_active_recs"] = [rec.unique_name for rec in non_default_active_recs] + + ########################################## + # faucet validation + if "faucets" in target_check_actions: + ( + has_faucets, + num_faucet_objs, + num_faucet_recs, + num_faucet_active_recs, + num_navigable_faucets, + ) = try_find_faucets(sim) + scene_test_results[sim.curr_scene_name]["has_faucets"] = has_faucets + scene_test_results[sim.curr_scene_name][ + "num_faucet_objs" + ] = num_faucet_objs + scene_test_results[sim.curr_scene_name][ + "num_faucet_recs" + ] = num_faucet_recs + scene_test_results[sim.curr_scene_name][ + "num_faucet_active_recs" + ] = num_faucet_active_recs + scene_test_results[sim.curr_scene_name][ + "num_navigable_faucets" + ] = num_navigable_faucets + + ########################################## + # Check region counts + 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 + 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 + 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" + ] = "" + for expected_region in missing_expected_regions: + scene_test_results[sim.curr_scene_name][ + "missing_expected_regions" + ] += f"{expected_region} | " + + ############################################## + # analyze semantics + # 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}__" + # ) + + csv_filepath = os.path.join(args.out_dir, "siro_scene_test_results.csv") + export_results_csv(csv_filepath, scene_test_results) + 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) diff --git a/tools/collision_shape_automation.py b/tools/collision_shape_automation.py new file mode 100644 index 0000000000..d279649fbe --- /dev/null +++ b/tools/collision_shape_automation.py @@ -0,0 +1,2633 @@ +# 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 ctypes +import math +import os +import random +import sys +import time +from typing import Any, Dict, List, Optional, Tuple + +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'") + +# not adding this causes some failures in mesh import +flags = sys.getdlopenflags() +sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) + + +# 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 +from habitat.sims.habitat_simulator.sim_utilities import snap_down + +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 + +# 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): + """ + Scales the icosphere_points for use with raycasting applications. + """ + 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. + Prints dict key->type recursively with incremental whitespace formatting. + """ + if whitespace == "": + print("-----------------------------------") + print("Print Dict Structure Results:") + for key in input_dict: + 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 + + +def compute_area_weights_for_range3d_faces(range3d: mn.Range3D): + """ + Compute a set of area weights from a Range3D. + """ + + 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] + + return normalized_area_accumulator + + +def get_range3d_sample_planes(range3d: mn.Range3D): + """ + Get origin and basis vectors for each face's sample planes. + """ + # 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 + ] + 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 = [] + 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 + + +# End - Range3D surface sampling utils +# ======================================================================= + + +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. + :param center: sphere center position + :param radius: sphere radius + :param num_points: number of points to sample + """ + 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 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, + 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 + 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, 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, 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 + + +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 = [] + + # draw first hits for gt and proxy + if gt_results.has_hits(): + 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 * ray_len + local_max_error = max(gt_dist, ray_len - gt_dist) + max_error += local_max_error + local_proxy_dist = ray_len + if proxy_results[r_ix].has_hits(): + 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 + return normalized_error + + +# =================================================================== +# 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], + output_directory: Optional[str] = None, + mm: Optional[habitat_sim.metadata.MetadataMediator] = None, + ) -> None: + # load the dataset into a persistent, shared MetadataMediator instance. + 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 + 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 + + # 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.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]] = {} + + # 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: + """ + 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 = self.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}`" + + # 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 + 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 + ) + + # 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 + 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 + 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 + rom = sim.get_rigid_object_manager() + obj = rom.add_object_by_template_handle(obj_handle) + assert obj.is_alive, "Object was not added correctly." + + 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, + ) + + # sample test points on the receptacles + for receptacle in obj_receptacles: + if type(receptacle) == hab_receptacle.TriangleMeshReceptacle: + rec_test_points = [] + t_radius = 0.01 + # adaptive density sample: + 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( + # receptacle.sample_uniform_global( + # sim, sample_region_scale=1.0 + # ) + # ) + self.gt_data[obj_handle]["receptacles"][receptacle.name] = { + "sample_points": rec_test_points, + "shape_id_results": {}, + } + if self.generate_debug_images: + debug_lines = [] + for face in range( + int(len(receptacle.mesh_data.indices) / 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(), + ) + ) + debug_circles = [] + for p in rec_test_points: + debug_circles.append( + ( + ( + p, # center + t_radius, # radius + mn.Vector3(0, 1, 0), # normal + mn.Color4.red(), # color + ) + ) + ) + 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, + 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 and 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="gt_" + ) + + # load a simulator instance with this object as the stage + 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 + ) + # 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": + # 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"] = 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." + + # 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"] = 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"] = 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." + + # when evaluating multiple proxy shapes, need unique ids: + pr_id = self.get_proxy_shape_id(obj_handle) + + # 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 and 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=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_id] = 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 Receptacle 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: + 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( + obj_handle + ) + assert obj.is_alive, "Object was not added correctly." + + # when evaluating multiple proxy shapes, need unique ids: + 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 + 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: 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: + 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_point_access_scores[ + receptacle_name + ] = sample_point_access_ratios + + receptacle_access_score /= len(sample_points) + receptacle_access_rate /= len(sample_points) + + 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" + ] = { + "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, + } + 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}") + + 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 = [] + 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( + ( + [ + 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"gt_{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"{shape_id}_{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"gt_{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"{shape_id}_{receptacle_name}_point_ratios_", + debug_lines=None, + debug_circles=debug_circles, + ) + # 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.1, + ): + """ + 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 receptacle to snapped point considered successful (meters) + """ + + constructed_cyl_obj_handle = self.construct_cylinder_object( + self.mm, cyl_radius, cyl_height + ) + + 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: Optional[hab_debug_vis.DebugVisualizer] = 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, + 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." + # 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 = self.get_proxy_shape_id(obj_handle) + + # 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." + + # 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: + sample_points = rec_data[rec_name]["sample_points"] + + 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() + # 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 + receptacle_sample_height_correction) + ).length() + - cyl_test_obj_com_height + ) + if expected_height_error > accepted_height_error: + failed_by_distance += 1 + point_stabilities.append(False) + 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.half_angle(cyl_test_obj.rotation, identity_q), + mn.math.half_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 + point_stabilities.append(False) + else: + point_stabilities.append(True) + else: + failed_snap += 1 + point_stabilities.append(False) + + 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 + + # write results to cache + if shape_id not in rec_data[rec_name]["shape_id_results"]: + rec_data[rec_name]["shape_id_results"][shape_id] = {} + assert ( + "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, + "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: + """ + 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. + 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 = 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() + 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: Optional[hab_debug_vis.DebugVisualizer] = 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, + 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 + 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: + 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 + 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.") + + 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, + "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 = 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] + 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 + + self.gt_data[obj_handle]["shape_test_results"][shape_id][ + "collision_grid_report" + ][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 = 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: + 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." + ) + + self.gt_data[obj_handle]["shape_test_results"][shape_id][ + "sphere_shake_report" + ] = { + "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. + 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 shape_id in self.gt_data[obj_handle]["raycasts"]: + self.setup_shape_test_results_cache(obj_handle, shape_id) + if ( + 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"], + self.gt_data[obj_handle]["raycasts"][shape_id], + ) + self.gt_data[obj_handle]["shape_test_results"][shape_id][ + "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, + param_range_override: Optional[Dict[str, List[Any]]] = None, + ) -> None: + """ + 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], + } + + if param_range_override is not None: + param_ranges = param_range_override + + 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}" + + 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) + + 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) + # 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]}" + ) + + 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). + """ + 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 + ] + 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: + 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 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: Optional[str] = None, + method="coacd", + param_range_override: Optional[Dict[str, List[Any]]] = None, + ): + """ + 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. + """ + 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) + if method == "coacd": + self.run_coacd_grid_search(obj_h, param_range_override) + self.compute_gt_errors(obj_h) + + # time to select the best version + best_shape_id = "pr0" + pr0_shape_score = self.compute_shape_score(obj_h, "pr0") + settings_key = method + "_settings" + best_shape_score = pr0_shape_score + shape_scores = {} + 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) + if ( + shape_score > (best_shape_score + abs(best_shape_score) * 0.1) + and shape_score - best_shape_score > 0.1 + ): + best_shape_id = shape_id + best_shape_score = shape_score + + print(self.gt_data[obj_h][settings_key]) + 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}." + ) + # copy the collision asset into the dataset directory + 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: + 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][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 cache_global_results(self) -> None: + """ + Cache the current global cumulative results. + Do this after an object's computation is done (compute_gt_errors) before cleaning the gt data. + """ + + 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] = { + "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" + ] + 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: + """ + Save current global results to a csv file in the self.output_directory. + """ + + 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) + + # 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: + if metric == "col_grid": + for subdiv in shape_results["col_grid"]: + 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 (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(existing_cols) + + # write results rows + 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) + + # 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: + if metric not in active_rec_metrics: + active_rec_metrics.append(metric) + + # 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 = ["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, 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) + + def compute_and_save_results_for_objects( + self, obj_handle_substrings: List[str], output_filename: str = "cpo_out" + ) -> 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) + + # 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) + + # receptacle metrics: + if self.compute_receptacle_useability_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 Receptacle Metrics:") + self.compute_receptacle_access_metrics(obj_h, use_gt=False) + self.compute_gt_errors(obj_h) + print_dict_structure(self.gt_data) + self.cache_global_results() + print_dict_structure(self.results) + self.clean_obj_gt(obj_h) + + # then save results to file + 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 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 + sim_settings["default_agent_navmesh"] = False + + 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 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: + 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 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." + ) + 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." + ) + group.add_argument( + "--all-rec-objects", + 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, + 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( + "--parts-only", + action="store_true", + help="culls all objects without _part_ in the name.", + ) + 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( + "--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, + 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.", + ) + parser.add_argument( + "--export-fp-model-ids", + type=str, + 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() + + 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 = { + "threshold": args.coacd_thresholds, + } + + 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 + 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) + cpo.generate_debug_images = args.debug_images + otm = cpo.mm.object_template_manager + + 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 or args.all_rec_objects or args.objects_file: + assert ( + not args.export_fp_model_ids + ), "Feature not available for objects, only for scenes." + + unique_objects = None + + 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 = [ + 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 = [] + for object_name in unique_objects: + # get templates matches + 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}" + 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) + + 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 = [] + 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_ix} of {len(object_handles)}") + print("+++++++++++++++++++++++++") + try: + results.append( + cpo.optimize_object_col_shape( + obj_h, + method="coacd", + 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): + print(f" {obj_h}: {obj_result}") + print("Failures:") + for f in failures: + print(f" {f}") + write_failure_ids(failures) + # ---------------------------------------------------- + + # ---------------------------------------------------- + # 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 + ) + assert ( + len(objects_in_scene) > 0 + ), f"No objects found in scene '{scene_name}'. Are you sure this is a valid 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 + + 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.values(): + 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]]] + ] = {} + 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)}" + ) + # ---------------------------------------------------- + + # ---------------------------------------------------- + # 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", param_range_override=param_range_overrides + ) + + 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__": + main() 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..422928c775 --- /dev/null +++ b/tools/generate_blend_to_urdf_parser_report.py @@ -0,0 +1,222 @@ +# 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_mesh.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, + ] + 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) + 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("-----------------------------------------------") + + print(f"global_counts = {global_count}") + + +if __name__ == "__main__": + main() diff --git a/tools/modify_scenes_config_properties.py b/tools/modify_scenes_config_properties.py new file mode 100644 index 0000000000..03b584e043 --- /dev/null +++ b/tools/modify_scenes_config_properties.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 json +import os +from typing import Any, Callable, Dict, 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 get_scene_instance_json(filepath: str) -> List[str]: + """ + Load a scene instance JSON as a dict and return it. + """ + assert filepath.endswith(".scene_instance.json"), "Must be a scene instance JSON." + + with open(filepath, "r") as f: + scene_conf = json.load(f) + return scene_conf + + +def save_scene_instance_json(filepath: str, scene_dict: Dict[str, Any]) -> None: + with open(filepath, "w") as f: + json.dump(scene_dict, f, indent=4) + + +def get_scene_id_from_filepath(filepath: str) -> str: + return filepath.split("/")[-1].split(".")[0] + + +def scene_has_semantic_map(scene_dict: Dict[str, Any]) -> bool: + if "semantic_scene_instance" in scene_dict: + if scene_dict["semantic_scene_instance"] == "hssd_ssd_map": + return True + return None + return None + + +def write_self_semantic(filepath: str) -> None: + scene_dict = get_scene_instance_json(filepath) + scene_id = get_scene_id_from_filepath(filepath) + scene_dict["semantic_scene_instance"] = f"{scene_id}.semantic_config.json" + save_scene_instance_json(filepath, scene_dict) + + +def clear_articulated_object_states(filepath: str) -> None: + scene_dict = get_scene_instance_json(filepath) + if "articulated_object_instances" in scene_dict: + for ao_instance in scene_dict["articulated_object_instances"]: + if "initial_joint_pose" in ao_instance: + del ao_instance["initial_joint_pose"] + if "initial_joint_velocities" in ao_instance: + del ao_instance["initial_joint_velocities"] + save_scene_instance_json(filepath, scene_dict) + + +def set_articulated_objects_motion_type( + filepath: str, motion_type: str = "static" +) -> None: + """ + Set all AOs in the scene instance to the specified motion type. + Choices are "static", "dynamic", or "kinematic" + """ + assert motion_type in ["static", "dynamic", "kinematic"] + scene_dict = get_scene_instance_json(filepath) + if "articulated_object_instances" in scene_dict: + for ao_instance in scene_dict["articulated_object_instances"]: + ao_instance["motion_type"] = motion_type + save_scene_instance_json(filepath, scene_dict) + + +def set_path_to_scene_filter_file(filepath: str, rel_filter_path: str) -> None: + """ + Sets the scene's receptacle filter path. + """ + + assert rel_filter_path.endswith(".json") + + scene_dict = get_scene_instance_json(filepath) + if "user_defined" not in scene_dict: + scene_dict["user_defined"] = {} + scene_dict["user_defined"]["scene_filter_file"] = rel_filter_path + save_scene_instance_json(filepath, scene_dict) + + +def clear_user_defined(filepath: str): + """ + Clear any user_defined subconfigs from a scene instance file leaving behind only the filter file. + """ + scene_dict = get_scene_instance_json(filepath) + if ( + "user_defined" in scene_dict + and "scene_filter_file" in scene_dict["user_defined"] + ): + # allow the filter filepath + scene_dict["user_defined"] = { + "scene_filter_file": scene_dict["user_defined"]["scene_filter_file"] + } + else: + del scene_dict["user_defined"] + save_scene_instance_json(filepath, scene_dict) + + +def main(): + parser = argparse.ArgumentParser( + description="Remove all 'semantic_scene_instance' fields from scene_instnace files in the dataset." + ) + parser.add_argument( + "--scenes-dir", + type=str, + help="Path to a directory containing the relevant '*.scene_dataset_config.json'. These will be overwritten.", + ) + parser.add_argument( + "--scenes", + nargs="+", + type=str, + help="One or more scene ids to limit modifications to a subset of the instance.", + ) + + args = parser.parse_args() + scenes_dir = args.scenes_dir + configs = find_files(scenes_dir, file_is_scene_config) + + if args.scenes is not None: + configs_subset = [] + for config in configs: + for scene_id in args.scenes: + if scene_id in config: + configs_subset.append(config) + break + configs = configs_subset + + # set path to scene_filter_files + if True: + for config in configs: + scene_name = config.split("/")[-1].split(".scene_instance.json")[0] + filter_path = f"scene_filter_files/siro_scene_filter_files/{scene_name}.rec_filter.json" + set_path_to_scene_filter_file(config, filter_path) + + # set all AOs to STATIC + if True: + for config in configs: + print(f"Setting ao motion_type for {config}") + set_articulated_objects_motion_type(config, motion_type="static") + + # remove articulated object states: + if True: + for config in configs: + print(f"Removing ao states from {config}") + clear_articulated_object_states(config) + + # remove everything except the filter file from the user_defined + if True: + for config in configs: + print(f"Clearing user_defined from {config}") + clear_user_defined(config) + + # semantic scene id modifier + if False: + num_w_semantic = 0 + for _ix, filepath in enumerate(configs): + write_self_semantic(filepath) + scene_dict = get_scene_instance_json(filepath) + if scene_has_semantic_map(scene_dict): + print(f"yes = {filepath}") + num_w_semantic += 1 + else: + print(f"no = {filepath}") + + print( + f"Total scenes with semantic specifier = {num_w_semantic} / {len(configs)}" + ) + + +if __name__ == "__main__": + main() 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() 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..e8e87622d4 --- /dev/null +++ b/tools/replace_articulated_models_in_rigid_scene.py @@ -0,0 +1,214 @@ +# 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, + 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. + + :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["rotation"] + 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(src_dir)[0] + dest_dir + filepath.split(src_dir)[-1] + with open(filepath, "w") as f: + json.dump(scene_conf, f, indent=4) + + +def main(): + parser = argparse.ArgumentParser( + description="Modify the scene_instance.json files, replacing rigid objects with articulated coutnerparts in a urdf/ directory." + ) + 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( + "--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="+", + 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 + 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, dest_dir), 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 valid_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, + src_dir=src_dir, + dest_dir=dest_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() diff --git a/troublesome_object_ids.txt b/troublesome_object_ids.txt new file mode 100644 index 0000000000..22bd7ba04b --- /dev/null +++ b/troublesome_object_ids.txt @@ -0,0 +1,3 @@ +1d5a78b46d32bf41584c800a0dfa2536d7f0b395 +05980eee8561a3ebaf0753a2f14f5871611e693e +0928513ee59d54e84c3baef6fe2f6daa7c9339b3