diff --git a/hand_image_failed_detection.png b/hand_image_failed_detection.png new file mode 100644 index 0000000000..60850e60d3 Binary files /dev/null and b/hand_image_failed_detection.png differ diff --git a/init_search_for_objects_angle0_back_fisheye_image.png b/init_search_for_objects_angle0_back_fisheye_image.png new file mode 100644 index 0000000000..2b3f03795f Binary files /dev/null and b/init_search_for_objects_angle0_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle0_frontleft_fisheye_image.png b/init_search_for_objects_angle0_frontleft_fisheye_image.png new file mode 100644 index 0000000000..f8ff67ef17 Binary files /dev/null and b/init_search_for_objects_angle0_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle0_frontright_fisheye_image.png b/init_search_for_objects_angle0_frontright_fisheye_image.png new file mode 100644 index 0000000000..f213712fda Binary files /dev/null and b/init_search_for_objects_angle0_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle0_hand_color_image.png b/init_search_for_objects_angle0_hand_color_image.png new file mode 100644 index 0000000000..5025b24539 Binary files /dev/null and b/init_search_for_objects_angle0_hand_color_image.png differ diff --git a/init_search_for_objects_angle0_left_fisheye_image.png b/init_search_for_objects_angle0_left_fisheye_image.png new file mode 100644 index 0000000000..539348987c Binary files /dev/null and b/init_search_for_objects_angle0_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle0_right_fisheye_image.png b/init_search_for_objects_angle0_right_fisheye_image.png new file mode 100644 index 0000000000..334849f2ea Binary files /dev/null and b/init_search_for_objects_angle0_right_fisheye_image.png differ diff --git a/init_search_for_objects_angle1_back_fisheye_image.png b/init_search_for_objects_angle1_back_fisheye_image.png new file mode 100644 index 0000000000..0859f437b0 Binary files /dev/null and b/init_search_for_objects_angle1_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle1_frontleft_fisheye_image.png b/init_search_for_objects_angle1_frontleft_fisheye_image.png new file mode 100644 index 0000000000..34f5419fff Binary files /dev/null and b/init_search_for_objects_angle1_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle1_frontright_fisheye_image.png b/init_search_for_objects_angle1_frontright_fisheye_image.png new file mode 100644 index 0000000000..af626121c8 Binary files /dev/null and b/init_search_for_objects_angle1_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle1_hand_color_image.png b/init_search_for_objects_angle1_hand_color_image.png new file mode 100644 index 0000000000..5e5a6792d0 Binary files /dev/null and b/init_search_for_objects_angle1_hand_color_image.png differ diff --git a/init_search_for_objects_angle1_left_fisheye_image.png b/init_search_for_objects_angle1_left_fisheye_image.png new file mode 100644 index 0000000000..7c8d7bc10c Binary files /dev/null and b/init_search_for_objects_angle1_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle1_right_fisheye_image.png b/init_search_for_objects_angle1_right_fisheye_image.png new file mode 100644 index 0000000000..97689f90a5 Binary files /dev/null and b/init_search_for_objects_angle1_right_fisheye_image.png differ diff --git a/init_search_for_objects_angle2_back_fisheye_image.png b/init_search_for_objects_angle2_back_fisheye_image.png new file mode 100644 index 0000000000..fe156f0de4 Binary files /dev/null and b/init_search_for_objects_angle2_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle2_frontleft_fisheye_image.png b/init_search_for_objects_angle2_frontleft_fisheye_image.png new file mode 100644 index 0000000000..ee883d991e Binary files /dev/null and b/init_search_for_objects_angle2_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle2_frontright_fisheye_image.png b/init_search_for_objects_angle2_frontright_fisheye_image.png new file mode 100644 index 0000000000..bb3cf50055 Binary files /dev/null and b/init_search_for_objects_angle2_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle2_hand_color_image.png b/init_search_for_objects_angle2_hand_color_image.png new file mode 100644 index 0000000000..70a9603726 Binary files /dev/null and b/init_search_for_objects_angle2_hand_color_image.png differ diff --git a/init_search_for_objects_angle2_left_fisheye_image.png b/init_search_for_objects_angle2_left_fisheye_image.png new file mode 100644 index 0000000000..fe0cc5025a Binary files /dev/null and b/init_search_for_objects_angle2_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle2_right_fisheye_image.png b/init_search_for_objects_angle2_right_fisheye_image.png new file mode 100644 index 0000000000..833270f93b Binary files /dev/null and b/init_search_for_objects_angle2_right_fisheye_image.png differ diff --git a/init_search_for_objects_angle3_back_fisheye_image.png b/init_search_for_objects_angle3_back_fisheye_image.png new file mode 100644 index 0000000000..b48d9f02ea Binary files /dev/null and b/init_search_for_objects_angle3_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle3_frontleft_fisheye_image.png b/init_search_for_objects_angle3_frontleft_fisheye_image.png new file mode 100644 index 0000000000..8cec9ed004 Binary files /dev/null and b/init_search_for_objects_angle3_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle3_frontright_fisheye_image.png b/init_search_for_objects_angle3_frontright_fisheye_image.png new file mode 100644 index 0000000000..2883475b62 Binary files /dev/null and b/init_search_for_objects_angle3_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle3_hand_color_image.png b/init_search_for_objects_angle3_hand_color_image.png new file mode 100644 index 0000000000..79fb0fd429 Binary files /dev/null and b/init_search_for_objects_angle3_hand_color_image.png differ diff --git a/init_search_for_objects_angle3_left_fisheye_image.png b/init_search_for_objects_angle3_left_fisheye_image.png new file mode 100644 index 0000000000..42b4fb697e Binary files /dev/null and b/init_search_for_objects_angle3_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle3_right_fisheye_image.png b/init_search_for_objects_angle3_right_fisheye_image.png new file mode 100644 index 0000000000..0923ceec7d Binary files /dev/null and b/init_search_for_objects_angle3_right_fisheye_image.png differ diff --git a/init_search_for_objects_angle4_back_fisheye_image.png b/init_search_for_objects_angle4_back_fisheye_image.png new file mode 100644 index 0000000000..1233bc6eec Binary files /dev/null and b/init_search_for_objects_angle4_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle4_frontleft_fisheye_image.png b/init_search_for_objects_angle4_frontleft_fisheye_image.png new file mode 100644 index 0000000000..a2368ae2ee Binary files /dev/null and b/init_search_for_objects_angle4_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle4_frontright_fisheye_image.png b/init_search_for_objects_angle4_frontright_fisheye_image.png new file mode 100644 index 0000000000..844de1fabf Binary files /dev/null and b/init_search_for_objects_angle4_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle4_hand_color_image.png b/init_search_for_objects_angle4_hand_color_image.png new file mode 100644 index 0000000000..6b2ad5364e Binary files /dev/null and b/init_search_for_objects_angle4_hand_color_image.png differ diff --git a/init_search_for_objects_angle4_left_fisheye_image.png b/init_search_for_objects_angle4_left_fisheye_image.png new file mode 100644 index 0000000000..34ca9cdb04 Binary files /dev/null and b/init_search_for_objects_angle4_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle4_right_fisheye_image.png b/init_search_for_objects_angle4_right_fisheye_image.png new file mode 100644 index 0000000000..79e042a2b5 Binary files /dev/null and b/init_search_for_objects_angle4_right_fisheye_image.png differ diff --git a/init_search_for_objects_angle5_back_fisheye_image.png b/init_search_for_objects_angle5_back_fisheye_image.png new file mode 100644 index 0000000000..f18a2fc192 Binary files /dev/null and b/init_search_for_objects_angle5_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle5_frontleft_fisheye_image.png b/init_search_for_objects_angle5_frontleft_fisheye_image.png new file mode 100644 index 0000000000..bede64067a Binary files /dev/null and b/init_search_for_objects_angle5_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle5_frontright_fisheye_image.png b/init_search_for_objects_angle5_frontright_fisheye_image.png new file mode 100644 index 0000000000..83299b7605 Binary files /dev/null and b/init_search_for_objects_angle5_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle5_hand_color_image.png b/init_search_for_objects_angle5_hand_color_image.png new file mode 100644 index 0000000000..1dcf7e91c1 Binary files /dev/null and b/init_search_for_objects_angle5_hand_color_image.png differ diff --git a/init_search_for_objects_angle5_left_fisheye_image.png b/init_search_for_objects_angle5_left_fisheye_image.png new file mode 100644 index 0000000000..4dc79892b0 Binary files /dev/null and b/init_search_for_objects_angle5_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle5_right_fisheye_image.png b/init_search_for_objects_angle5_right_fisheye_image.png new file mode 100644 index 0000000000..a469fc8c99 Binary files /dev/null and b/init_search_for_objects_angle5_right_fisheye_image.png differ diff --git a/init_search_for_objects_angle6_back_fisheye_image.png b/init_search_for_objects_angle6_back_fisheye_image.png new file mode 100644 index 0000000000..152cbbc22b Binary files /dev/null and b/init_search_for_objects_angle6_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle6_frontleft_fisheye_image.png b/init_search_for_objects_angle6_frontleft_fisheye_image.png new file mode 100644 index 0000000000..97b2415d80 Binary files /dev/null and b/init_search_for_objects_angle6_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle6_frontright_fisheye_image.png b/init_search_for_objects_angle6_frontright_fisheye_image.png new file mode 100644 index 0000000000..b75a35e6d9 Binary files /dev/null and b/init_search_for_objects_angle6_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle6_hand_color_image.png b/init_search_for_objects_angle6_hand_color_image.png new file mode 100644 index 0000000000..33277431f5 Binary files /dev/null and b/init_search_for_objects_angle6_hand_color_image.png differ diff --git a/init_search_for_objects_angle6_left_fisheye_image.png b/init_search_for_objects_angle6_left_fisheye_image.png new file mode 100644 index 0000000000..e309318050 Binary files /dev/null and b/init_search_for_objects_angle6_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle6_right_fisheye_image.png b/init_search_for_objects_angle6_right_fisheye_image.png new file mode 100644 index 0000000000..71b162b1aa Binary files /dev/null and b/init_search_for_objects_angle6_right_fisheye_image.png differ diff --git a/init_search_for_objects_angle7_back_fisheye_image.png b/init_search_for_objects_angle7_back_fisheye_image.png new file mode 100644 index 0000000000..8aa7c43820 Binary files /dev/null and b/init_search_for_objects_angle7_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle7_frontleft_fisheye_image.png b/init_search_for_objects_angle7_frontleft_fisheye_image.png new file mode 100644 index 0000000000..eb4ed8c636 Binary files /dev/null and b/init_search_for_objects_angle7_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle7_frontright_fisheye_image.png b/init_search_for_objects_angle7_frontright_fisheye_image.png new file mode 100644 index 0000000000..98acc99f5a Binary files /dev/null and b/init_search_for_objects_angle7_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle7_hand_color_image.png b/init_search_for_objects_angle7_hand_color_image.png new file mode 100644 index 0000000000..db7531b5a3 Binary files /dev/null and b/init_search_for_objects_angle7_hand_color_image.png differ diff --git a/init_search_for_objects_angle7_left_fisheye_image.png b/init_search_for_objects_angle7_left_fisheye_image.png new file mode 100644 index 0000000000..2884f6fa14 Binary files /dev/null and b/init_search_for_objects_angle7_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle7_right_fisheye_image.png b/init_search_for_objects_angle7_right_fisheye_image.png new file mode 100644 index 0000000000..dbde624a11 Binary files /dev/null and b/init_search_for_objects_angle7_right_fisheye_image.png differ diff --git a/init_search_for_objects_angle8_back_fisheye_image.png b/init_search_for_objects_angle8_back_fisheye_image.png new file mode 100644 index 0000000000..2d23dcf6ce Binary files /dev/null and b/init_search_for_objects_angle8_back_fisheye_image.png differ diff --git a/init_search_for_objects_angle8_frontleft_fisheye_image.png b/init_search_for_objects_angle8_frontleft_fisheye_image.png new file mode 100644 index 0000000000..7a2459a0cd Binary files /dev/null and b/init_search_for_objects_angle8_frontleft_fisheye_image.png differ diff --git a/init_search_for_objects_angle8_frontright_fisheye_image.png b/init_search_for_objects_angle8_frontright_fisheye_image.png new file mode 100644 index 0000000000..c9bdf9aef7 Binary files /dev/null and b/init_search_for_objects_angle8_frontright_fisheye_image.png differ diff --git a/init_search_for_objects_angle8_hand_color_image.png b/init_search_for_objects_angle8_hand_color_image.png new file mode 100644 index 0000000000..34c438d5ea Binary files /dev/null and b/init_search_for_objects_angle8_hand_color_image.png differ diff --git a/init_search_for_objects_angle8_left_fisheye_image.png b/init_search_for_objects_angle8_left_fisheye_image.png new file mode 100644 index 0000000000..cbb7e06f72 Binary files /dev/null and b/init_search_for_objects_angle8_left_fisheye_image.png differ diff --git a/init_search_for_objects_angle8_right_fisheye_image.png b/init_search_for_objects_angle8_right_fisheye_image.png new file mode 100644 index 0000000000..89fd823566 Binary files /dev/null and b/init_search_for_objects_angle8_right_fisheye_image.png differ diff --git a/predicators/approaches/bilevel_planning_approach.py b/predicators/approaches/bilevel_planning_approach.py index cb8ca38347..4f67894bad 100644 --- a/predicators/approaches/bilevel_planning_approach.py +++ b/predicators/approaches/bilevel_planning_approach.py @@ -72,6 +72,11 @@ def _solve(self, task: Task, timeout: int) -> Callable[[State], Action]: for act in nsrt_plan: logging.debug(act) + for act in nsrt_plan: + print(act) + import ipdb + ipdb.set_trace() + # Run full bilevel planning. else: option_plan, nsrt_plan, metrics = self._run_sesame_plan( diff --git a/predicators/approaches/documentation/grammar_search_invention_approach.md b/predicators/approaches/documentation/grammar_search_invention_approach.md index 7d629f0d78..c527e747c0 100644 --- a/predicators/approaches/documentation/grammar_search_invention_approach.md +++ b/predicators/approaches/documentation/grammar_search_invention_approach.md @@ -62,7 +62,7 @@ apple_coring__vlm_demos__456__2 | options.txt ### Running predicate invention using these image demos -To use the Gemini VLM, you need to set the `GOOGLE_API_KEY` environment variable in your terminal. You can make/get an API key [here](https://aistudio.google.com/app/apikey). +To use the Gemini VLM, you need to set the `GOOGLE_API_KEY` environment variable in your terminal. You can make/get an API key [here](https://aistudio.google.com/app/apikey). To use an Open AI VLM, the `OPENAI_API_KEY` variable needs to be set. Example command: `python predicators/main.py --env apple_coring --seed 456 --approach grammar_search_invention --excluded_predicates all --num_train_tasks 1 --num_test_tasks 0 --offline_data_method saved_vlm_img_demos_folder --vlm_trajs_folder_name apple_coring__vlm_demos__456__1` diff --git a/predicators/approaches/grammar_search_invention_approach.py b/predicators/approaches/grammar_search_invention_approach.py index 5197ce575f..46ca37e95f 100644 --- a/predicators/approaches/grammar_search_invention_approach.py +++ b/predicators/approaches/grammar_search_invention_approach.py @@ -1021,9 +1021,8 @@ def _parse_atom_dataset_from_annotated_dataset( for ground_atom in ground_atom_state: assert isinstance(ground_atom, GroundAtom) if ground_atom.predicate not in candidates: - # The cost of this predicate is simply its arity. - candidates[ground_atom.predicate] = float( - len(ground_atom.objects)) + candidates[ground_atom.predicate] = float( + len(ground_atom.objects)) logging.debug(f"All candidate predicates: {candidates.keys()}") return (atom_dataset, candidates) diff --git a/predicators/approaches/spot_wrapper_approach.py b/predicators/approaches/spot_wrapper_approach.py index 85af72d148..2ba741a398 100644 --- a/predicators/approaches/spot_wrapper_approach.py +++ b/predicators/approaches/spot_wrapper_approach.py @@ -18,6 +18,7 @@ from predicators import utils from predicators.approaches import BaseApproach, BaseApproachWrapper from predicators.envs.spot_env import get_detection_id_for_object, get_robot +from predicators.settings import CFG from predicators.spot_utils.skills.spot_find_objects import find_objects from predicators.spot_utils.skills.spot_stow_arm import stow_arm from predicators.spot_utils.utils import get_allowed_map_regions @@ -66,23 +67,24 @@ def _policy(state: State) -> Action: state.get(obj, "lost") > 0.5: lost_objects.add(obj) # Need to find the objects. - if lost_objects: - logging.info(f"[Spot Wrapper] Lost objects: {lost_objects}") - # Reset the base approach policy. - base_approach_policy = None - need_stow = True - self._base_approach_has_control = False - robot, localizer, lease_client = get_robot() - lost_object_ids = { - get_detection_id_for_object(o) - for o in lost_objects - } - allowed_regions = self._allowed_regions - extra_info = SpotActionExtraInfo( - "find-objects", [], find_objects, - (state, self._rng, robot, localizer, lease_client, - lost_object_ids, allowed_regions), None, tuple()) - return utils.create_spot_env_action(extra_info) + # NOTE: HACK: commenting out for now - just for some robot testing! + # if lost_objects and len(CFG.spot_vlm_teleop_demo_folderpath) == 0: + # logging.info(f"[Spot Wrapper] Lost objects: {lost_objects}") + # # Reset the base approach policy. + # base_approach_policy = None + # need_stow = True + # self._base_approach_has_control = False + # robot, localizer, lease_client = get_robot() + # lost_object_ids = { + # get_detection_id_for_object(o) + # for o in lost_objects + # } + # allowed_regions = self._allowed_regions + # extra_info = SpotActionExtraInfo( + # "find-objects", [], find_objects, + # (state, self._rng, robot, localizer, lease_client, + # lost_object_ids, allowed_regions), None, tuple()) + # return utils.create_spot_env_action(extra_info) # Found the objects. Stow the arm before replanning. if need_stow: logging.info("[Spot Wrapper] Lost objects found, stowing.") diff --git a/predicators/approaches/vlm_open_loop_approach.py b/predicators/approaches/vlm_open_loop_approach.py index 79e9f12cd6..590545aaf8 100644 --- a/predicators/approaches/vlm_open_loop_approach.py +++ b/predicators/approaches/vlm_open_loop_approach.py @@ -67,6 +67,39 @@ def get_name(cls) -> str: def is_learning_based(self) -> bool: return True + def get_goals_for_specific_datasets(self, train_task_idx: int) -> str: + """HACK to get the goals for specific datasets. + + Used for spot envs because the invention env is different from + the actual execution env. + """ + if CFG.vlm_trajs_folder_name == "spot_vlm_table_wiping_execution_env__vlm_demos__3__8": + if train_task_idx in [0, 2, 3]: + return "TableWiped(child_play_table:table)" + elif train_task_idx in [1, 4]: + return "VLMIn(apple:movable, seethru_plastic_dustbin:movable), TableWiped(child_play_table:table)" + elif train_task_idx == 5: + return "VLMIn(green_block: movable, cardboard_recycling_bin:movable)" + elif train_task_idx == 6: + return "VLMIn(orange_block: movable, cardboard_recycling_bin:movable)" + elif train_task_idx == 7: + return "VLMIn(spam_tin:movable, cardboard_recycling_bin:movable)" + else: + raise NotImplementedError( + "Shouldn't be getting here! i = {}".format(i)) + elif CFG.vlm_trajs_folder_name == "spot_vlm_table_wiping_human_execution_env__vlm_demos__3__6": + if train_task_idx in [0, 1, 2]: + return "TableWiped(child_play_table:table)" + elif train_task_idx in [3]: + return "VLMIn(apple:movable, seethru_plastic_dustbin:movable), TableWiped(child_play_table:table)" + elif train_task_idx == 4: + return "VLMIn(green_block: movable, cardboard_recycling_bin:movable)" + elif train_task_idx == 5: + return "VLMIn(spam_tin:movable, cardboard_recycling_bin:movable)" + else: + raise NotImplementedError( + "Shouldn't be getting here! i = {}".format(i)) + def learn_from_offline_dataset(self, dataset: Dataset) -> None: """Adds the images and plans from the training dataset to the base prompt for use at test time!""" @@ -77,7 +110,7 @@ def _append_to_prompt_state_imgs_list(state: State) -> None: for img_num, img in enumerate(state.simulator_state["images"]): pil_img = PIL.Image.fromarray(img) # type: ignore width, height = pil_img.size - font_size = 15 + font_size = 6 text = f"Demonstration {traj_num}, " + \ f"State {state_num}, Image {img_num}" draw = ImageDraw.Draw(pil_img) @@ -119,9 +152,16 @@ def _append_to_prompt_state_imgs_list(state: State) -> None: segment_traj, ll_traj = seg_traj if not ll_traj.is_demo: continue - traj_goal = self._train_tasks[ll_traj.train_task_idx].goal + + # Get the goal string depending on the env. + if "spot" not in CFG.env: + traj_goal = self._train_tasks[ll_traj.train_task_idx].goal + traj_goal_str = str(sorted(traj_goal)) + else: + traj_goal_str = self.get_goals_for_specific_datasets(traj_num) + self._prompt_demos_str += f"Demonstration {traj_num}, " + \ - f"Goal: {str(sorted(traj_goal))}\n" + f"Goal: {traj_goal_str}\n" assert len(segment_traj) > 0 for state_num, seg in enumerate(segment_traj): state = seg.states[0] @@ -162,14 +202,20 @@ def _query_vlm_for_option_plan(self, task: Task) -> Sequence[_Option]: assert isinstance(init_state.simulator_state["images"], List) curr_options = sorted(self._initial_options) imgs = init_state.simulator_state["images"] - pil_imgs = [ - PIL.Image.fromarray(img_arr) # type: ignore - for img_arr in imgs - ] + if isinstance(imgs[0], np.ndarray): + pil_imgs = [ + PIL.Image.fromarray(img_arr) # type: ignore + for img_arr in imgs + ] + elif isinstance(imgs[0], PIL.Image.Image): + pil_imgs = imgs + else: + raise ValueError( + "Simulator state images are not in a recognized format!") imgs_for_vlm = [] for img_num, pil_img in enumerate(pil_imgs): draw = ImageDraw.Draw(pil_img) - img_font = utils.get_scaled_default_font(draw, 10) + img_font = utils.get_scaled_default_font(draw, 6) img_with_txt = utils.add_text_to_draw_img( draw, (50, 50), f"Initial state to plan from, Image {img_num}", img_font) diff --git a/predicators/datasets/__init__.py b/predicators/datasets/__init__.py index 220259d737..fd7b0270bd 100644 --- a/predicators/datasets/__init__.py +++ b/predicators/datasets/__init__.py @@ -9,7 +9,8 @@ from predicators.datasets.generate_atom_trajs_with_vlm import \ create_ground_atom_data_from_generated_demos, \ create_ground_atom_data_from_labelled_txt, \ - create_ground_atom_data_from_saved_img_trajs + create_ground_atom_data_from_saved_img_trajs, \ + create_low_level_trajs_from_saved_img_trajs from predicators.datasets.ground_atom_data import create_ground_atom_data from predicators.envs import BaseEnv from predicators.settings import CFG @@ -77,7 +78,7 @@ def create_dataset(env: BaseEnv, train_tasks: List[Task], "demo+labelled_atoms", "geo_and_demo+labelled_atoms" ]: return create_ground_atom_data_from_labelled_txt( - env, train_tasks, known_options) + env, train_tasks, known_options, known_predicates=known_predicates) if CFG.offline_data_method in [ "saved_vlm_img_demos_folder", "geo_and_saved_vlm_img_demos_folder" ]: # pragma: no cover. @@ -87,6 +88,9 @@ def create_dataset(env: BaseEnv, train_tasks: List[Task], # we want to instantiate our own 'dummy' VLM. return create_ground_atom_data_from_saved_img_trajs( env, train_tasks, known_predicates, known_options) + if CFG.offline_data_method == "saved_vlm_img_demos_folder_nolabel": + return create_low_level_trajs_from_saved_img_trajs( + env, train_tasks, known_predicates, known_options) if CFG.offline_data_method == "empty": return Dataset([]) raise NotImplementedError("Unrecognized dataset method.") diff --git a/predicators/datasets/generate_atom_trajs_with_vlm.py b/predicators/datasets/generate_atom_trajs_with_vlm.py index 925b0ff626..0e51d37d4e 100644 --- a/predicators/datasets/generate_atom_trajs_with_vlm.py +++ b/predicators/datasets/generate_atom_trajs_with_vlm.py @@ -5,6 +5,7 @@ import itertools import logging import os +import random import re import textwrap import traceback @@ -13,8 +14,8 @@ from functools import partial from inspect import getsource from pathlib import Path -from typing import Dict, Iterator, List, Match, Optional, Sequence, Set, \ - Tuple, cast +from typing import Collection, Dict, Iterator, List, Match, Optional, \ + Sequence, Set, Tuple, cast import dill as pkl import numpy as np @@ -29,7 +30,7 @@ from predicators.settings import CFG from predicators.structs import Action, Dataset, GroundAtom, \ ImageOptionTrajectory, LowLevelTrajectory, Object, ParameterizedOption, \ - Predicate, State, Task, _Option + Predicate, State, Task, Type, VLMPredicate, _Option def _generate_prompt_for_atom_proposals( @@ -78,14 +79,93 @@ def _generate_prompt_for_atom_proposals( for act in traj.actions) # NOTE: exact same issue as described in the above note for # naive_whole_traj. - ret_list.append( - (prompt, [traj.imgs[i][0] for i in range(len(traj.imgs))])) + try: + ret_list.append( + (prompt, [traj.imgs[i][0] for i in range(len(traj.imgs))])) + except IndexError: + import ipdb; ipdb.set_trace() else: # pragma: no cover. raise ValueError("Unknown VLM prompting option " + f"{CFG.grammar_search_vlm_atom_proposal_prompt_type}") return ret_list +def _parse_known_vlm_atoms_from_saved_traj( + types: Collection[Type], + train_tasks: Sequence[Task]) -> List[List[Set[GroundAtom]]]: + """Checks if the folder where the ground atoms trajectories are saved has a + file with known predicates, and subfiles with atom values for each state in + each trajctory. + + If it does, we add these to the other ground atoms in the + trajectories. + """ + ground_atoms_trajs = [[] for _ in range(len(train_tasks))] + trajectories_folder_path = os.path.join( + utils.get_path_to_predicators_root(), CFG.data_dir, + CFG.vlm_trajs_folder_name) + known_preds_file_path = os.path.join(trajectories_folder_path, + "known_predicates.txt") + env_type_name_to_type = {t.name: t for t in types} + if not os.path.exists(known_preds_file_path): + return ground_atoms_trajs + # Parse predicates from the known_predicates.txt file + known_pred_name_to_pred: Dict[str, Predicate] = {} + with open(known_preds_file_path, "r", encoding="utf-8") as f: + line = f.readline().strip() + assert line.startswith("Predicates:") + predicates_str = line[len("Predicates:"):].strip() + predicates = predicates_str.split("; ") + for predicate in predicates: + name_and_args = predicate.split("(") + pred_name = name_and_args[0].strip() + args = name_and_args[1].strip(")").split(", ") + arg_types = [arg.split(": ")[1] for arg in args] + curr_types = [ + env_type_name_to_type[arg_type] for arg_type in arg_types + ] + pred = Predicate(pred_name, curr_types, lambda s, o: False) + known_pred_name_to_pred[pred_name] = pred + # Now, for each trajectory, parse out atom values from the + # known_predicate_vals.txt file. + unfiltered_paths = sorted(Path(trajectories_folder_path).iterdir()) + filtered_paths = [f for f in unfiltered_paths if "traj_" in f.parts[-1]] + for train_task_idx, path in enumerate(filtered_paths): + assert path.is_dir() + known_pred_vals_filepath = os.path.join(path, + "known_predicate_vals.txt") + assert os.path.exists(known_pred_vals_filepath) + with open(known_pred_vals_filepath, "r", encoding="utf-8") as f: + curr_task = train_tasks[train_task_idx] + obj_name_to_obj = {obj.name: obj for obj in set(curr_task.init)} + for line in f: + line = line.strip() + if not line: + continue + timestep, atom_strs = line.split(": ", 1) + timestep = int(timestep) + # Split multiple atoms by commas + atom_list = [atom.strip() for atom in atom_strs.split(";")] + for atom_str in atom_list: + predicate_name = atom_str.split("(")[0].strip() + args_str = atom_str.split("(")[1].strip(")") + args = [arg.strip() for arg in args_str.split(",")] + pred = known_pred_name_to_pred[predicate_name] + ground_objects = [obj_name_to_obj[arg] for arg in args] + try: + ground_atom = GroundAtom(pred, ground_objects) + except AssertionError: + import ipdb + ipdb.set_trace() + # Ensure the ground_atoms_trajs list is large enough + while len(ground_atoms_trajs[train_task_idx]) <= timestep: + ground_atoms_trajs[train_task_idx].append(set()) + # Add the ground atom to the appropriate timestep + ground_atoms_trajs[train_task_idx][timestep].add( + ground_atom) + return ground_atoms_trajs + + def _generate_prompt_for_scene_labelling( traj: ImageOptionTrajectory, atoms_list: List[str], label_history: List[str] @@ -125,6 +205,8 @@ def _sample_vlm_atom_proposals_from_trajectories( 0.0, CFG.seed, num_completions=1)) + # print(aggregated_vlm_output_strs[0][0]) + # import ipdb; ipdb.set_trace() curr_num_queries += 1 logging.info("Completed (%s/%s) init atoms queries to the VLM.", curr_num_queries, total_num_queries) @@ -148,7 +230,7 @@ def _label_single_trajectory_with_vlm_atom_values(indexed_traj: Tuple[ atom_objs = atom_args.split(',') keep = True for ao in atom_objs: - if ao not in obj_names: + if ao.replace(" ", "") not in obj_names: keep = False continue if keep: @@ -182,7 +264,10 @@ def _label_single_trajectory_with_vlm_atom_values(indexed_traj: Tuple[ "/predicators/datasets/vlm_input_data_prompts/atom_labelling/" + \ "double_check_prompt_prev_labels.txt" # pylint: enable=line-too-long - double_check_prompt += previous_timestep_check_prompt + with open(previous_timestep_check_prompt, "r", + encoding="utf-8") as f: + previous_timestep_check_prompt_str = f.read() + double_check_prompt += previous_timestep_check_prompt_str double_check_prompt += "\n\nTruth values of predicates at " + \ "the previous timestep:\n\n" @@ -282,6 +367,9 @@ def _parse_unique_atom_proposals_from_list( all_atom_groundings = set() unique_predicates = set() obj_names_set = set(obj.name for obj in relevant_objects_across_demos) + # NOTE: just for human invention env! + if "human_invention" in CFG.env: + obj_names_set = set(obj.name for obj in relevant_objects_across_demos if "robot" not in str(obj.type)) # We'll use these mappings to generate VLM atoms for every possible # grounding of each proposed predicate. @@ -292,7 +380,6 @@ def _parse_unique_atom_proposals_from_list( type_to_obj_names = defaultdict(list) for obj_name, _type in obj_name_to_type.items(): type_to_obj_names[_type].append(obj_name) - num_atoms_considered = 0 for atoms_proposal_for_traj in atom_strs_proposals_list: assert len(atoms_proposal_for_traj) == 1 @@ -409,7 +496,9 @@ def _save_img_option_trajs_in_folder( curr_traj_timestep_folder = Path(curr_traj_folder, str(j)) os.makedirs(curr_traj_timestep_folder, exist_ok=False) for k, img in enumerate(img_list): - img.save( + # Convert RGBA to RGB since JPEG doesn't support alpha + img_to_save = img.convert('RGB') if img.mode == 'RGBA' else img + img_to_save.save( Path(curr_traj_timestep_folder, str(j) + "_" + str(k) + ".jpg")) # Save the object-centric state alongside the images. @@ -459,6 +548,17 @@ def _get_vlm_query_str(pred_name: str, objects: Sequence[Object]) -> str: pred_name_and_obj_types_to_pred = {} atoms_trajs = [] + + # Start by adding any VLM predicates in the goal to + # pred_name_and_obj_types_to_pred. + for pred in known_predicates: + if isinstance(pred, VLMPredicate): + # NOTE: IMPORTANT: we assume that the predicate name and + # its VLM query string are identical. + pred_name_and_obj_types_str = pred.name + "(" + ",".join( + str(obj_type.name) for obj_type in pred.types) + ")" + pred_name_and_obj_types_to_pred[pred_name_and_obj_types_str] = pred + # Loop through all trajectories in the structured_state_trajs and convert # each one to a sequence of sets of GroundAtoms. for i, traj in enumerate(structured_state_trajs): @@ -467,13 +567,22 @@ def _get_vlm_query_str(pred_name: str, objects: Sequence[Object]) -> str: curr_obj_name_to_obj = {obj.name: obj for obj in objs_for_task} # If we have states, then we can just evaluate the goal predicates on # them. But if we don't, then there's nothing we can do except assume - # that there is only one goal atom that gets satisfied at the end. + # (1) that there is only one goal atom that gets satisfied at the end + # or (2) that we are using VLM-based goal predicates assume_goal_holds_at_end = use_dummy_goal if state_trajs is None: - assert len(train_tasks[i].goal) == 1 - assert known_predicates is None or \ - known_predicates.issubset(env.goal_predicates) - assume_goal_holds_at_end = True + for goal_atom in train_tasks[i].goal: + if not isinstance(goal_atom.predicate, VLMPredicate): + break + else: + assume_goal_holds_at_end = True + if not assume_goal_holds_at_end: + assert len(train_tasks[i].goal) == 1 + goal_atom = list(train_tasks[i].goal)[0] + assert goal_atom.predicate.name == "DummyGoal" + assert known_predicates is None or \ + known_predicates.issubset(env.goal_predicates) + assume_goal_holds_at_end = True if use_dummy_goal: # NOTE: In this case, we assume that there is precisely one dummy @@ -753,16 +862,56 @@ def _generate_ground_atoms_with_vlm_pure_visual_preds( atom_proposals_set = _parse_unique_atom_proposals_from_list( atom_strs_proposals_list, all_task_objs) assert len(atom_proposals_set) > 0, "Atom proposals set is empty!" + # If any of the known predicates are VLM predicates, we want to add + # these to the set of atom proposals. + for pred in known_predicates: + if isinstance(pred, VLMPredicate): + all_ground_atoms_for_pred = utils.get_all_ground_atoms_for_predicate( + pred, all_task_objs) + ground_atoms_vlm_query_strs = set( + atom.get_vlm_query_str() for atom in all_ground_atoms_for_pred) + # NOTE: technically we only need to add an arbitrary grounding, but this + # grounding might be too specific (e.g. if all objects of type `table` are + # also of type `immovable` due to hierarchy, and if we arbitrarily ground + # with a table, then we'll propose the wrong predicate...) + atom_proposals_set |= ground_atoms_vlm_query_strs + # Given this set of unique atom proposals, we now ask the VLM # to label these in every scene from the demonstrations. # NOTE: we convert to a sorted list here to get rid of randomness from set # ordering. unique_atoms_list = sorted(atom_proposals_set) + + # # We now randomly take a subset for CFG.grammar_search_max_predicates. + # if len(unique_atoms_list) > CFG.grammar_search_max_predicates: + # rng = np.random.default_rng(CFG.seed) + # unique_atoms_list = rng.choice(unique_atoms_list, CFG.grammar_search_max_predicates) + # logging.info("VLM atom proposals set is too large, subsampling down to " + # f"{CFG.grammar_search_max_predicates} predicates.") + # Now, query the VLM! logging.info("Querying VLM to label every scene...") atom_labels = _label_trajectories_with_vlm_atom_values( image_option_trajs, vlm, unique_atoms_list) logging.info("Done querying VLM for scene labelling!") + # Now parse out any known predicates that might exist. + parsed_known_ground_atoms_trajs = _parse_known_vlm_atoms_from_saved_traj( + env.types, train_tasks) + # Combine these known ground atoms into the VLM labelled atoms. + # NOTE: this is slightly inefficient - because the known ground atoms are + # already parsed into GroundAtoms, but we're turning them back into + # strings here. However, that doesn't matter: saving them as strings is + # quite important. + for i, ground_atoms_traj in enumerate(parsed_known_ground_atoms_trajs): + for j, ground_atoms in enumerate(ground_atoms_traj): + known_ground_atoms_str = "" + for ground_atom in ground_atoms: + known_ground_atoms_str += "\n* " + str( + ground_atom.predicate) + "(" + ", ".join( + obj.name + for obj in ground_atom.objects) + ")" + ": True." + atom_labels[i][j] += known_ground_atoms_str + # Save the output as a human-readable txt file. _save_labelled_trajs_as_txt( env, atom_labels, [io_traj.actions for io_traj in image_option_trajs]) @@ -1116,7 +1265,8 @@ def create_ground_atom_data_from_generated_demos( def create_ground_atom_data_from_labelled_txt( env: BaseEnv, train_tasks: List[Task], - known_options: Set[ParameterizedOption]) -> Dataset: + known_options: Set[ParameterizedOption], + known_predicates: Set[Predicate]) -> Dataset: """Given a txt file containing trajectories labelled with VLM predicate values, construct a dataset that can be passed to the rest of our learning pipeline.""" @@ -1128,7 +1278,7 @@ def create_ground_atom_data_from_labelled_txt( # Next, take this intermediate structured form and further # parse it into ground atoms and ground options respectively. ground_atoms_trajs = _parse_structured_state_into_ground_atoms( - env, train_tasks, structured_states) + env, train_tasks, structured_states, known_predicates=known_predicates) _debug_log_atoms_trajs(ground_atoms_trajs) option_trajs = _parse_structured_actions_into_ground_options( structured_actions, known_options, train_tasks) @@ -1190,7 +1340,10 @@ def create_ground_atom_data_from_saved_img_trajs( # Each demonstration trajectory is in subfolder traj_. traj_folders = [f for f in unfiltered_files if f[0:5] == "traj_"] num_trajs = len(traj_folders) - assert num_trajs == CFG.num_train_tasks + try: + assert num_trajs == CFG.num_train_tasks + except AssertionError: + import ipdb; ipdb.set_trace() option_name_to_option = {opt.name: opt for opt in known_options} image_option_trajs = [] all_task_objs = set() @@ -1228,7 +1381,7 @@ def create_ground_atom_data_from_saved_img_trajs( all_task_objs |= curr_task_objs curr_task_obj_name_to_obj = {obj.name: obj for obj in curr_task_objs} # Parse out actions for the trajectory. - options_traj_file_list = glob.glob(str(path) + "/*.txt") + options_traj_file_list = glob.glob(str(path) + "/*options_traj.txt") assert len(options_traj_file_list) == 1 options_traj_file = options_traj_file_list[0] with open(options_traj_file, "r", encoding="utf-8") as f: @@ -1260,16 +1413,13 @@ def create_ground_atom_data_from_saved_img_trajs( # Now actually create ground options. for option_name, option_objs_strs_list, option_params in zip( option_names_list, object_args_list, parameters): + option = option_name_to_option[option_name] objects = [ curr_task_obj_name_to_obj[opt_arg] for opt_arg in option_objs_strs_list ] - option = option_name_to_option[option_name] - if isinstance(option_params, float): - params_tuple = (option_params, ) - else: - params_tuple = option_params - ground_option = option.ground(objects, np.array(params_tuple)) + params = np.zeros(option.params_space.shape) + ground_option = option.ground(objects, params) assert ground_option.initiable(curr_train_task.init) ground_option_traj.append(ground_option) # Given ground options, we can finally make ImageOptionTrajectories. @@ -1302,7 +1452,7 @@ def create_ground_atom_data_from_saved_img_trajs( low_level_trajs = _convert_ground_option_trajs_into_lowleveltrajs( [traj.actions for traj in image_option_trajs], goal_states_for_every_traj, train_tasks) - else: + elif image_option_trajs[0].states is not None: low_level_trajs = [] for io_traj in image_option_trajs: assert io_traj.states is not None @@ -1311,4 +1461,178 @@ def create_ground_atom_data_from_saved_img_trajs( Action(np.zeros(env.action_space.shape, dtype=np.float32), act) for act in io_traj.actions ], True, io_traj.train_task_idx)) + else: + # Here, the goal consists of VLM predicates: just make the goal + # state the same as the initial state! + goal_states_for_every_traj = [ + train_tasks[i].init for i in range(len(train_tasks)) + ] + low_level_trajs = _convert_ground_option_trajs_into_lowleveltrajs( + [traj.actions for traj in image_option_trajs], + goal_states_for_every_traj, train_tasks) return Dataset(low_level_trajs, ground_atoms_trajs) + + +def create_low_level_trajs_from_saved_img_trajs( + env: BaseEnv, train_tasks: List[Task], + known_predicates: Set[Predicate], + known_options: Set[ParameterizedOption]) -> Dataset: + """Given a folder containing trajectories that have images of scenes for + each state, as well as options that transition between these states, output + a dataset. + + Importantly - unlike the above method - this does not actually label + atom values. It just creates a dataset of low-level trajectories + (i.e. trajectories of actions and states). + """ + trajectories_folder_path = os.path.join( + utils.get_path_to_predicators_root(), CFG.data_dir, + CFG.vlm_trajs_folder_name) + # # First, run some checks on the folder name to make sure + # # we're not accidentally loading the wrong one. + # folder_name_components = CFG.vlm_trajs_folder_name.split('__') + # assert folder_name_components[0] == CFG.env + # assert folder_name_components[1] == "vlm_demos" + # assert int(folder_name_components[2]) == CFG.seed + # assert int(folder_name_components[3]) == CFG.num_train_tasks + unfiltered_files = os.listdir(trajectories_folder_path) + # Each demonstration trajectory is in subfolder traj_. + traj_folders = [f for f in unfiltered_files if f[0:5] == "traj_"] + num_trajs = len(traj_folders) + assert num_trajs == CFG.num_train_tasks + option_name_to_option = {opt.name: opt for opt in known_options} + image_option_trajs = [] + all_task_objs = set() + unfiltered_paths = sorted(Path(trajectories_folder_path).iterdir()) + # Each demonstration trajectory is in subfolder traj_. + filtered_paths = [f for f in unfiltered_paths if "traj_" in f.parts[-1]] + for train_task_idx, path in enumerate(filtered_paths): + assert path.is_dir() + state_folders = [f.path for f in os.scandir(path) if f.is_dir()] + num_states_in_traj = len(state_folders) + img_traj = [] + state_traj: Optional[List[State]] = [] + for state_num in range(num_states_in_traj): + curr_imgs: List[PIL.Image.Image] = [] + curr_state_path = path.joinpath(str(state_num)) + # NOTE: we assume all images are saved as jpg files. + img_files = sorted(glob.glob(str(curr_state_path) + "/*.jpg")) + for img_file in img_files: + # PIL.Image.open returns an ImageFile, which is a subclass of + # an Image. + img = cast(PIL.Image.Image, PIL.Image.open(img_file)) + curr_imgs.append(img) + img_traj.append(curr_imgs) + state_file = curr_state_path / "state.p" + if state_file.exists(): # pragma: no cover + with open(state_file, "rb") as fp: + state = pkl.load(fp) + assert state_traj is not None + state_traj.append(state) + else: + state_traj = None + # Get objects from train tasks to be used for future parsing. + curr_train_task = train_tasks[train_task_idx] + curr_task_objs = set(curr_train_task.init) + all_task_objs |= curr_task_objs + curr_task_obj_name_to_obj = {obj.name: obj for obj in curr_task_objs} + # Parse out actions for the trajectory. + options_traj_file_list = glob.glob(str(path) + "/*options_traj.txt") + assert len(options_traj_file_list) == 1 + options_traj_file = options_traj_file_list[0] + with open(options_traj_file, "r", encoding="utf-8") as f: + options_file_str = f.read() + option_names_list = re.findall(r'(\w+)\(', options_file_str) + option_args_strs = re.findall(r'\((.*?)\)', options_file_str) + parsed_str_objects = [ + re.sub(r'\[[^\]]*\]', '', option_args_str).strip() + for option_args_str in option_args_strs + ] + objects_exist = len(''.join(obj_str + for obj_str in parsed_str_objects)) > 0 + object_args_list: List[List[str]] = [ + [] for _ in range(len(parsed_str_objects)) + ] + if objects_exist: + cleaned_parsed_str_objects = [ + obj_str[:-1] if obj_str[-1] == "," else obj_str + for obj_str in parsed_str_objects + ] + object_args_list = [ + obj.split(', ') for obj in cleaned_parsed_str_objects + ] + parameters = [ + ast.literal_eval(obj) if obj else [] + for obj in re.findall(r'\[(.*?)\]', options_file_str) + ] + ground_option_traj: List[_Option] = [] + # Now actually create ground options. + for option_name, option_objs_strs_list, option_params in zip( + option_names_list, object_args_list, parameters): + option = option_name_to_option[option_name] + if "spot" not in CFG.env: + objects = [ + curr_task_obj_name_to_obj[opt_arg] + for opt_arg in option_objs_strs_list + ] + else: + # In the case of spot environments, teh state doesn't + # have the objects directly. We have to make them + # up as we go. + objects = [] + for i, obj_name in enumerate(option_objs_strs_list): + objects.append(Object(obj_name, option.types[i])) + + if "spot" not in CFG.env: + if isinstance(option_params, float): + params_tuple = (option_params, ) + else: + params_tuple = option_params + ground_option = option.ground(objects, np.array(params_tuple)) + else: + params = np.zeros(option.params_space.shape) + ground_option = option.ground(objects, params) + assert ground_option.initiable(curr_train_task.init) + ground_option_traj.append(ground_option) + # Given ground options, we can finally make ImageOptionTrajectories. + image_option_trajs.append( + ImageOptionTrajectory(list(curr_task_objs), + img_traj, [], + ground_option_traj, + state_traj, + _is_demo=True, + _train_task_idx=train_task_idx)) + # Finally, we just need to construct LowLevelTrajectories that we can + # output as part of our Dataset. + assert "DummyGoal" not in str(train_tasks[0].goal) + # Finally, we need to construct actual LowLevelTrajectories. + # NOTE: In this LowLevelTrajectory, we assume the low level states + # are the same as the init state until the final state. + trajs = [] + for traj_num in range(len(image_option_trajs)): + traj_init_state = train_tasks[traj_num].init + curr_traj_states = [] + curr_traj_actions = [] + curr_img_traj = image_option_trajs[traj_num].imgs + for idx_within_traj in range(len( + image_option_trajs[traj_num].actions)): + curr_state = traj_init_state.copy() + curr_state.simulator_state["images"] = [ + np.array(img) for img in curr_img_traj[idx_within_traj] + ] + curr_traj_states.append(curr_state) + curr_traj_actions.append( + Action(np.zeros(0, dtype=float), + image_option_trajs[traj_num].actions[idx_within_traj])) + # Now, we need to append the final state because there are 1 more + # states than actions. + curr_state = traj_init_state.copy() + curr_state.simulator_state["images"] = [ + np.array(img) for img in curr_img_traj[-1] + ] + curr_traj_states.append(curr_state) + curr_traj = LowLevelTrajectory(curr_traj_states, curr_traj_actions, + True, traj_num) + trajs.append(curr_traj) + + return Dataset(trajs) diff --git a/predicators/datasets/vlm_input_data_prompts/atom_labelling/img_option_diffs_label_history.txt b/predicators/datasets/vlm_input_data_prompts/atom_labelling/img_option_diffs_label_history.txt index 28df26f020..36955452f3 100644 --- a/predicators/datasets/vlm_input_data_prompts/atom_labelling/img_option_diffs_label_history.txt +++ b/predicators/datasets/vlm_input_data_prompts/atom_labelling/img_option_diffs_label_history.txt @@ -1,27 +1,7 @@ -You are a vision system for a robot. You are provided with two images corresponding to the states before and after a particular skill is executed. You are given a list of predicates below, and you are given the values of these predicates in the image before the skill is executed. Your job is to output the values of the following predicates in the image after the skill is executed. Pay careful attention to the visual changes between the two images to figure out which predicates change and which predicates do not change. Note that some or all of the predicates don't necessary have to change. First, output a description of what changes you expect to happen based on the skill that was just run, explicitly noting the skill that was run. Second, output a description of what visual changes you see happen between the before and after images, looking specifically at the objects involved in the skill's arguments, noting what objects these are. Next, output each predicate value in the after image as a bulleted list (use '*' for the bullets) with each predicate and value on a different line. Ensure there is a period ('.') after the truth value of the predicate. For each predicate value, provide an explanation as to why you labelled this predicate as having this particular value. Use the format: `* : . `. When labeling the value of a predicate, if you don't see the objects involved in that predicate, retain its truth value from the previous timestep. - -The current task plan involves the following actions in sequence: -TeleopPick1, (robot:robot, dustpan:dustpan) -PlaceNextTo, (robot:robot, dustpan:dustpan, wrappers:wrappers) -TeleopPick2, (robot:robot, broom:broom) -Sweep, (robot:robot, broom:broom, wrappers:wrappers, dustpan:dustpan) -PlaceOnFloor, (robot:robot, broom:broom) -TeleopPick1, (robot:robot, dustpan:dustpan) - -Your response should have three sections. Here is an outline of what your response should look like: -[START OULTLINE] -# Expected changes based on the executed skill -[insert your analysis on the expected changes you will see based on the skill that was executed] - -# Visual changes observed between the images -[insert your analysis on the visual changes observed between the images] - -# Predicate values in the after image -[insert your bulleted list of `* : . `] -[END OUTLINE] +You are a vision system for a robot. You are provided with two images corresponding to the states before and after a particular skill is executed. You are given a list of predicates below, and you are given the values of these predicates in the image before the skill is executed. Your job is to output the values of the following predicates in the image after the skill is executed. Pay careful attention to the visual changes between the two images to figure out which predicates change and which predicates do not change. For the predicates that change, list these separately at the end of your response. Note that in some scenes, there might be no changes. First, output a description of what changes you expect to happen based on the skill that was just run, explicitly noting the skill that was run. Second, output a description of what visual changes you see happen between the before and after images, looking specifically at the objects involved in the skill's arguments, noting what objects these are. Next, output each predicate value in the after image as a bulleted list with each predicate and value on a different line. For each predicate value *that changes*, provide an explanation as to why you labelled this predicate as having this particular value (no explanation is necessary if you believe the value has not changed). Use the format: : . . Your response should have three sections. Here is an outline of what your response should look like: -[START OULTLINE] +[START OUTLINE] # Expected changes based on the executed skill [insert your analysis on the expected changes you will see based on the skill that was executed] diff --git a/predicators/datasets/vlm_input_data_prompts/atom_labelling/img_option_diffs_label_history_burger.txt b/predicators/datasets/vlm_input_data_prompts/atom_labelling/img_option_diffs_label_history_burger.txt index 0ff574ad0b..0d69b59651 100644 --- a/predicators/datasets/vlm_input_data_prompts/atom_labelling/img_option_diffs_label_history_burger.txt +++ b/predicators/datasets/vlm_input_data_prompts/atom_labelling/img_option_diffs_label_history_burger.txt @@ -1,7 +1,7 @@ You are a vision system for a robot provided with four images: a before image showing the state before a skill is executed, an after image showing the state after the skill is executed, and two cropped images highlighting objects relevant to the skill's arguments (the third image is a crop of the before image, and the fourth image is a crop of the after image). You are given a list of predicates below, and you are given the values of these predicates in the image before the skill is executed. Your job is to output the values of the following predicates in the image after the skill is executed. Pay careful attention to the visual changes between the two images to figure out which predicates change and which predicates do not change. Note that some or all of the predicates don't necessary have to change. First, output a description of what changes you expect to happen based on the skill that was just run, explicitly noting the skill that was run. Second, output a description of what visual changes you see happen between the before and after images, looking specifically at the objects involved in the skill's arguments, noting what objects these are. From these two descriptions, for each predicate labeled in the previous timestep, note whether you expect its value to change or stay the same. Next, output each predicate value in the after image as a bulleted list (use '*' for the bullets) with each predicate and value on a different line. Ensure there is a period ('.') after the truth value of the predicate. For each predicate value, provide an explanation as to why you labelled this predicate as having this particular value, and note what value this predicate had in the previous timestep, which is given to you in the prompt. Use the format: `* : . `. When labeling the value of a predicate, if you don't see the objects involved in that predicate, retain its truth value from the previous timestep. If an object is not visible in the cropped images, check the uncropped images. Also, if your description of changes you expect to happen, and your description of visual changes you saw happen, have nothing to do with the predicate you are trying to label, retain its truth value from the previous timestep. For example, if in the previous timestep I paint an object, and in the current timestamp I sit on it, we don't expect its color to change after sitting on it. Note that when a cook action is taken on any kind of food that is on the grill, it does not take time to finish cooking -- it cooks instantly. Note that just placing any kind of food on the grill without explicitly cooking it does not make it cooked. Your response should have three sections. Here is an outline of what your response should look like: -[START OULTLINE] +[START OUTLINE] # Expected changes based on the executed skill [insert your analysis on the expected changes you will see based on the skill that was executed] diff --git a/predicators/datasets/vlm_input_data_prompts/atom_labelling/per_scene_cot.txt b/predicators/datasets/vlm_input_data_prompts/atom_labelling/per_scene_cot.txt index 74e1e05ac1..19068f730b 100644 --- a/predicators/datasets/vlm_input_data_prompts/atom_labelling/per_scene_cot.txt +++ b/predicators/datasets/vlm_input_data_prompts/atom_labelling/per_scene_cot.txt @@ -1,2 +1,4 @@ -You are a vision system for a robot. Your job is to output the values of the following predicates based on the provided visual scene. For each predicate, output True, False, or Unknown if the relevant objects are not in the scene or the value of the predicate simply cannot be determined. Output each predicate value as a bulleted list with each predicate and value on a different line. For each output value, provide an explanation as to why you labelled this predicate as having this particular value.Use the format: : . . +You are a vision system for a robot. Your job is to output the values of the following predicates based on the provided visual scene. +Start by first describing the visual scene given all the provided images - which might be from different cameras looking at the same underlying scene - in pure natural language. +For each predicate, output True, False, or Unknown if the relevant objects are not in the scene or the value of the predicate simply cannot be determined. Output each predicate value as a bulleted list with each predicate and value on a different line. For each output value, provide an explanation as to why you labelled this predicate as having this particular value. Use the format: : . . Predicates: \ No newline at end of file diff --git a/predicators/datasets/vlm_input_data_prompts/atom_proposal/options_labels_whole_traj_diverse_spot.txt b/predicators/datasets/vlm_input_data_prompts/atom_proposal/options_labels_whole_traj_diverse_spot.txt new file mode 100644 index 0000000000..7f1b397750 --- /dev/null +++ b/predicators/datasets/vlm_input_data_prompts/atom_proposal/options_labels_whole_traj_diverse_spot.txt @@ -0,0 +1,7 @@ +You are a robotic vision system whose job is to output a structured set of predicates useful for describing important concepts in the following demonstration of a task. You will be provided with a list of actions used during the task, as well as images of states before and after every action execution. Please provide predicates in terms of the following objects: {objs}. For each predicate, output it in the following format: predicate_name(obj1, obj2, obj3...). +Start by providing an overview description of the demonstration and the changes you observe in it, as well as a list of what concepts you think are relevant. +For each action, generate both "dynamic" predicates (i.e., those that you expect/see to change value before and after the action invocation) and "static" predicates (i.e., those that you expect/see stay constant, but are still important preconditions of the action and need to hold for the action to be correctly executed). +Finally, generate any additional predicates that might be important to describing the overall demonstration. Try to provide any synonyms and antonyms for the most important static and dynamic predicates: the more ways to describe a concept, the better! +Think very carefully about what relations/conditions (in the form of predicates) might be important for each of the particular actions to be executed. +When generating these predicate names, try to be as unambiguous as possible. For instance, instead of generating a predicate `Free(table)` to express the concept that there are no objects/clutter on the table, instead generate `NoObjectsOnTop(table)`, since this is far more precise. +Be careful to only use the objects listed above. DO NOT introduce any additional objects or concepts; if you want to do this, just change the predicate name. For instance, if you want to express the concept `IsColor(crayon, red)`, but the allowed object set only contains `crayon`, then this would be invalid (since there is no `red` object); instead you should output `IsColorRed(crayon)`. \ No newline at end of file diff --git a/predicators/envs/spot_env.py b/predicators/envs/spot_env.py index 911ff34dd4..0a951ce939 100644 --- a/predicators/envs/spot_env.py +++ b/predicators/envs/spot_env.py @@ -6,7 +6,7 @@ import time from dataclasses import dataclass from pathlib import Path -from typing import Callable, ClassVar, Collection, Dict, Iterator, List, \ +from typing import Any, Callable, ClassVar, Collection, Dict, Iterator, List, \ Optional, Sequence, Set, Tuple import matplotlib @@ -27,7 +27,8 @@ LanguageObjectDetectionID, ObjectDetectionID, _query_detic_sam, \ detect_objects, visualize_all_artifacts from predicators.spot_utils.perception.object_specific_grasp_selection import \ - brush_prompt, bucket_prompt, football_prompt, train_toy_prompt + brush_prompt, bucket_prompt, football_prompt, train_toy_prompt, blue_toy_chair_prompt, \ + orange_bucket_prompt from predicators.spot_utils.perception.perception_structs import RGBDImage, \ RGBDImageWithContext, SegmentedBoundingBox from predicators.spot_utils.perception.spot_cameras import capture_images, \ @@ -42,8 +43,8 @@ from predicators.spot_utils.spot_localization import SpotLocalizer from predicators.spot_utils.utils import _base_object_type, _broom_type, \ _container_type, _dustpan_type, _immovable_object_type, \ - _movable_object_type, _robot_type, _wrappers_type, \ - construct_state_given_pbrspot, get_allowed_map_regions, \ + _movable_object_type, _robot_type, _table_type, _trash_can_type, \ + _wrappers_type, construct_state_given_pbrspot, get_allowed_map_regions, \ get_graph_nav_dir, get_robot_gripper_open_percentage, get_spot_home_pose, \ load_spot_metadata, object_to_top_down_geom, update_pbrspot_given_state, \ update_pbrspot_robot_conf, verify_estop @@ -84,6 +85,9 @@ class _SpotObservation: # A placeholder until all predicates have classifiers nonpercept_atoms: Set[GroundAtom] nonpercept_predicates: Set[Predicate] + # Object detections per camera in self.images. + object_detections_per_camera: Dict[str, List[Tuple[ObjectDetectionID, + SegmentedBoundingBox]]] @dataclass(frozen=True) @@ -140,10 +144,8 @@ def allclose(self, other: State) -> bool: def copy(self) -> State: state_copy = {o: self._copy_state_value(self.data[o]) for o in self} - sim_state_copy = { - "predicates": self._simulator_state_predicates.copy(), - "atoms": self._simulator_state_atoms.copy() - } + if self.simulator_state is not None: + sim_state_copy = self.simulator_state.copy() return _PartialPerceptionState(state_copy, simulator_state=sim_state_copy) @@ -437,6 +439,11 @@ def _get_next_dry_observation( if action_name in ["find-objects", "stow-arm"]: return _dry_simulate_noop(obs, nonpercept_atoms) + if action_name == "WipeTable": + # The SurfaceWiped effect is handled via nonpercept_atoms. + # No other state changes needed for dry simulation. + return _dry_simulate_noop(obs, nonpercept_atoms) + raise NotImplementedError("Dry simulation not implemented for action " f"{action_name}") @@ -758,7 +765,18 @@ def _build_realworld_observation( break if response == "n": break - + # Construct the detections per camera. + obj_detections_per_camera: Dict[str, + List[Tuple[ObjectDetectionID, + SegmentedBoundingBox]]] = { + k: [] + for k in rgbds.keys() + } + for object_id, d in all_artifacts['language'][ + 'object_id_to_img_detections'].items(): + for camera_name, seg_bb in d.items(): + obj_detections_per_camera[camera_name].append( + (object_id, seg_bb)) # Prepare the non-percepts. nonpercept_preds = self.predicates - self.percept_predicates assert all(a.predicate in nonpercept_preds for a in ground_atoms) @@ -766,7 +784,8 @@ def _build_realworld_observation( objects_in_hand_view, objects_in_any_view_except_back, self._spot_object, gripper_open_percentage, - robot_pos, ground_atoms, nonpercept_preds) + robot_pos, ground_atoms, nonpercept_preds, + obj_detections_per_camera) return obs @@ -857,7 +876,15 @@ def _actively_construct_env_task(self) -> EnvironmentTask: # an initial observation. assert self._robot is not None assert self._localizer is not None - objects_in_view = self._actively_construct_initial_object_views() + objects_in_view, artifacts = self._actively_construct_initial_object_views( + ) + # ############### + # # HACK: just for ViLA running! + # objects_in_view = {} + # artifacts = {} + # artifacts['language'] = {} + # artifacts['language']['object_id_to_img_detections'] = {} + # ############### rgbd_images = capture_images(self._robot, self._localizer) gripper_open_percentage = get_robot_gripper_open_percentage( self._robot) @@ -866,9 +893,20 @@ def _actively_construct_env_task(self) -> EnvironmentTask: nonpercept_atoms = self._get_initial_nonpercept_atoms() nonpercept_preds = self.predicates - self.percept_predicates assert all(a.predicate in nonpercept_preds for a in nonpercept_atoms) + # Construct the detections per camera. + obj_detections_per_camera: Dict[str, List[Tuple[ + ObjectDetectionID, + SegmentedBoundingBox]]] = {k: [] + for k in rgbd_images.keys()} + for object_id, d in artifacts['language'][ + 'object_id_to_img_detections'].items(): + for camera_name, seg_bb in d.items(): + obj_detections_per_camera[camera_name].append( + (object_id, seg_bb)) obs = _SpotObservation(rgbd_images, objects_in_view, set(), set(), self._spot_object, gripper_open_percentage, - robot_pos, nonpercept_atoms, nonpercept_preds) + robot_pos, nonpercept_atoms, nonpercept_preds, + obj_detections_per_camera) goal_description = self._generate_goal_description() task = EnvironmentTask(obs, goal_description) # Save the task for future use. @@ -985,41 +1023,40 @@ def _load_task_from_json(self, json_file: Path) -> EnvironmentTask: # Prepare the non-percepts. nonpercept_atoms = self._get_initial_nonpercept_atoms() nonpercept_preds = self.predicates - self.percept_predicates - init_obs = _SpotObservation( - images, - objects_in_view, - set(), - set(), - robot, - gripper_open_percentage, - robot_pos, - nonpercept_atoms, - nonpercept_preds, - ) + init_obs = _SpotObservation(images, objects_in_view, set(), set(), + robot, gripper_open_percentage, robot_pos, + nonpercept_atoms, nonpercept_preds, {}) # The goal can remain the same. goal = base_env_task.goal_description return EnvironmentTask(init_obs, goal) def _actively_construct_initial_object_views( - self) -> Dict[Object, math_helpers.SE3Pose]: + self) -> Tuple[Dict[Object, math_helpers.SE3Pose], Dict[str, Any]]: assert self._robot is not None assert self._localizer is not None stow_arm(self._robot) - go_home(self._robot, self._localizer) + # go_home(self._robot, self._localizer) self._localizer.localize() detection_ids = self._detection_id_to_obj.keys() - detections = self._run_init_search_for_objects(set(detection_ids)) + detections, artifacts = self._run_init_search_for_objects( + set(detection_ids)) stow_arm(self._robot) obj_to_se3_pose = { self._detection_id_to_obj[det_id]: val for (det_id, val) in detections.items() } self._last_known_object_poses.update(obj_to_se3_pose) - return obj_to_se3_pose + # Move the robot into a good place to construct the initial state + # by running VLM predicates. + prompt = "Finished initial search for objects. Take control of the robot and move it into a good initial location for constructing the initial state of the task. Press 'Enter' when done!" + _ = input(prompt) + assert self._lease_client is not None + self._lease_client.take() + return obj_to_se3_pose, artifacts def _run_init_search_for_objects( self, detection_ids: Set[ObjectDetectionID] - ) -> Dict[ObjectDetectionID, math_helpers.SE3Pose]: + ) -> Tuple[Dict[ObjectDetectionID, math_helpers.SE3Pose], Dict[str, Any]]: """Have the hand look down from high up at first.""" assert self._robot is not None assert self._localizer is not None @@ -1041,7 +1078,8 @@ def _run_init_search_for_objects( no_detections_outfile = outdir / f"no_detections_{time_str}.png" visualize_all_artifacts(artifacts, detections_outfile, no_detections_outfile) - return detections + self._lease_client + return detections, artifacts @property @abc.abstractmethod @@ -1062,7 +1100,7 @@ def _generate_goal_description(self) -> GoalDescription: ############################################################################### ## Constants -HANDEMPTY_GRIPPER_THRESHOLD = 2.5 # made public for use in perceiver +HANDEMPTY_GRIPPER_THRESHOLD = 2.7 #2.5 # made public for use in perceiver _ONTOP_Z_THRESHOLD = 0.2 _INSIDE_Z_THRESHOLD = 0.3 _ONTOP_SURFACE_BUFFER = 0.48 @@ -1070,16 +1108,13 @@ def _generate_goal_description(self) -> GoalDescription: _FITS_IN_XY_BUFFER = 0.05 _REACHABLE_THRESHOLD = 0.925 # slightly less than length of arm _REACHABLE_YAW_THRESHOLD = 0.95 # higher better -_CONTAINER_SWEEP_READY_BUFFER = 0.35 +_CONTAINER_SWEEP_READY_BUFFER = 0.7 #0.35 _ROBOT_SWEEP_READY_TOL = 0.25 ## Types _ALL_TYPES = { - _robot_type, - _base_object_type, - _movable_object_type, - _immovable_object_type, - _container_type, + _robot_type, _base_object_type, _movable_object_type, + _immovable_object_type, _container_type, _table_type, _trash_can_type } @@ -1093,6 +1128,8 @@ def _neq_classifier(state: State, objects: Sequence[Object]) -> bool: def _handempty_classifier(state: State, objects: Sequence[Object]) -> bool: spot = objects[0] gripper_open_percentage = state.get(spot, "gripper_open_percentage") + if gripper_open_percentage > HANDEMPTY_GRIPPER_THRESHOLD: + print("GRIPPER_OPEN_PERCENTAGE", gripper_open_percentage) return gripper_open_percentage <= HANDEMPTY_GRIPPER_THRESHOLD @@ -1102,6 +1139,15 @@ def _holding_classifier(state: State, objects: Sequence[Object]) -> bool: return False return state.get(obj, "held") > 0.5 +def _open_classifier(state: State, objects: Sequence[Object]) -> bool: + obj = objects[0] + return False + +def _not_open_classifier(state: State, objects: Sequence[Object]) -> bool: + obj = objects[0] + if not obj.is_instance(_movable_object_type): + return True + return not _open_classifier(state, objects) def _not_holding_classifier(state: State, objects: Sequence[Object]) -> bool: _, obj = objects @@ -1138,6 +1184,15 @@ def _object_in_xy_classifier(state: State, def _on_classifier(state: State, objects: Sequence[Object]) -> bool: obj_on, obj_surface = objects + if "bear_toy" in obj_on.name and "panda_toy" in obj_surface.name: + return False + if "panda_toy" in obj_on.name and "bear_toy" in obj_surface.name: + return False + if "chair" in obj_on.name and "table" in obj_surface.name: + return False + if "table" in obj_on.name and "chair" in obj_surface.name: + return False + # Check that the bottom of the object is close to the top of the surface. expect = state.get(obj_surface, "z") + state.get(obj_surface, "height") / 2 actual = state.get(obj_on, "z") - state.get(obj_on, "height") / 2 @@ -1315,6 +1370,15 @@ def _blocking_classifier(state: State, objects: Sequence[Object]) -> bool: put_on_robot_if_held=False) ret_val = blocker_geom.intersects(blocked_robot_line) + + if "chair" in blocker_obj.name and "table" in blocked_obj.name: + print("DEBUG: considering chair to be blocking table", ret_val) + print("DEBUG: blocker geom", blocker_geom) + print("DEBUG: blocked_robot_line", blocked_robot_line) + print("DEBUG: blocker obj", blocker_obj) + print("DEBUG: blocked obj", blocked_obj) + print("DEBUG: robot_x", robot_x, "robot_y", robot_y, "blocked_x", blocked_x, "blocked_y", blocked_y, "blocker_x", state.get(blocker_obj, "x"), "blocker_y", state.get(blocker_obj, "y")) + return ret_val @@ -1361,6 +1425,10 @@ def _container_adjacent_to_surface_for_sweeping(container: Object, dist = np.sqrt((expected_x - container_x)**2 + (expected_y - container_y)**2) + if dist > _CONTAINER_SWEEP_READY_BUFFER and "table" in surface.name: + print( + f"DEBUG: Container {container.name} is not adjacent to surface {surface.name} for sweeping. Distance: {dist:.2f}" + ) return dist <= _CONTAINER_SWEEP_READY_BUFFER @@ -1386,6 +1454,20 @@ def _is_sweeper_classifier(state: State, objects: Sequence[Object]) -> bool: return state.get(obj, "is_sweeper") > 0.5 +def _surface_wiped_classifier(state: State, objects: Sequence[Object]) -> bool: + """SurfaceWiped is a non-percept predicate managed via simulator state. + + The state is updated by operator effects when WipeTable is executed. + """ + assert isinstance(state, _PartialPerceptionState) + # Find the SurfaceWiped predicate from the simulator state predicates + for pred in state._simulator_state_predicates: + if pred.name == "SurfaceWiped": + atom = GroundAtom(pred, objects) + return state.simulator_state_atom_holds(atom) + return False + + def _has_flat_top_surface_classifier(state: State, objects: Sequence[Object]) -> bool: obj, = objects @@ -1425,6 +1507,10 @@ def _get_sweeping_surface_for_container(container: Object, return None +_Open = Predicate("Open", [_movable_object_type], + _open_classifier) +_NotOpen = Predicate("NotOpen", [_movable_object_type], + _not_open_classifier) _NEq = Predicate("NEq", [_base_object_type, _base_object_type], _neq_classifier) _On = Predicate("On", [_movable_object_type, _base_object_type], @@ -1470,6 +1556,8 @@ def _get_sweeping_surface_for_container(container: Object, _RobotReadyForSweeping = Predicate("RobotReadyForSweeping", [_robot_type, _movable_object_type], _robot_ready_for_sweeping_classifier) +_SurfaceWiped = Predicate("SurfaceWiped", [_immovable_object_type], + _surface_wiped_classifier) _IsSemanticallyGreaterThan = Predicate( "IsSemanticallyGreaterThan", [_base_object_type, _base_object_type], _is_semantically_greater_than_classifier) @@ -1483,6 +1571,24 @@ def _get_vlm_query_str(pred_name: str, objects: Sequence[Object]) -> str: _VLMOn = utils.create_vlm_predicate("VLMOn", [_movable_object_type, _base_object_type], lambda o: _get_vlm_query_str("OnTopOf", o)) +_VLMOnTable = utils.create_vlm_predicate( + "VLMOnTable", [_movable_object_type, _table_type], + lambda o: _get_vlm_query_str("OnTopTable", o)) +_VLMOnFloor = utils.create_vlm_predicate( + "VLMOnFloor", [_movable_object_type], + lambda o: _get_vlm_query_str("OnFloor", o)) +_TableClear = utils.create_vlm_predicate( + "TableClear", [_table_type], + lambda o: _get_vlm_query_str("ClearOfObjects", o)) +_CanBeUsedForErasing = utils.create_vlm_predicate( + "CanBeUsedForErasing", [_base_object_type], + lambda o: _get_vlm_query_str("CanBeUsedForErasing", o)) +_TableWiped = utils.create_vlm_predicate( + "TableWiped", [_table_type], + lambda o: _get_vlm_query_str("WipedOfMarkerScribbles", o)) +_TableClean = utils.create_vlm_predicate( + "TableClean", [_table_type], + lambda o: _get_vlm_query_str("CleanOfObjectsAndMarkings", o)) _Upright = utils.create_vlm_predicate( "Upright", [_movable_object_type], lambda o: _get_vlm_query_str("Upright", o)) @@ -1491,7 +1597,7 @@ def _get_vlm_query_str(pred_name: str, objects: Sequence[Object]) -> str: lambda o: _get_vlm_query_str("Toasted", o)) _VLMIn = utils.create_vlm_predicate( "VLMIn", [_movable_object_type, _immovable_object_type], - lambda o: _get_vlm_query_str("In", o)) + lambda o: _get_vlm_query_str("Inside", o)) _Open = utils.create_vlm_predicate("Open", [_movable_object_type], lambda o: _get_vlm_query_str("Open", o)) _Stained = utils.create_vlm_predicate( @@ -1509,19 +1615,25 @@ def _get_vlm_query_str(pred_name: str, objects: Sequence[Object]) -> str: _Holding, _NotHolding, _InHandView, _InView, _Reachable, _Blocking, _NotBlocked, _ContainerReadyForSweeping, _IsPlaceable, _IsNotPlaceable, _IsSweeper, _HasFlatTopSurface, _RobotReadyForSweeping, - _IsSemanticallyGreaterThan, _Inside + _IsSemanticallyGreaterThan, _Inside, _Open, _NotOpen, _SurfaceWiped } _VLM_PREDICATES = { _VLMOn, + _VLMOnTable, + _VLMOnFloor, + _TableClear, + _TableWiped, + _TableClean, _Upright, _Toasted, + _CanBeUsedForErasing, _VLMIn, _Open, _Stained, _Messy, _Touching, } -_NONPERCEPT_PREDICATES: Set[Predicate] = set() +_NONPERCEPT_PREDICATES: Set[Predicate] = {_SurfaceWiped} ## Operators (needed in the environment for non-percept atom hack) @@ -1743,19 +1855,57 @@ def _create_operators() -> Iterator[STRIPSOperator]: parameters = [robot, blocker, blocked] preconds = { LiftedAtom(_NotBlocked, [blocked]), - LiftedAtom(_HandEmpty, [robot]), LiftedAtom(_Holding, [robot, blocker]), } add_effs = { LiftedAtom(_Blocking, [blocker, blocked]), + LiftedAtom(_HandEmpty, [robot]), LiftedAtom(_NotHolding, [robot, blocker]), } del_effs = { LiftedAtom(_Holding, [robot, blocker]), } - ignore_effs = {_InHandView, _Reachable, _RobotReadyForSweeping, _Blocking} + ignore_effs = {_InHandView, _Reachable, _RobotReadyForSweeping, _NotBlocked} yield STRIPSOperator("DragToBlockObject", parameters, preconds, add_effs, del_effs, ignore_effs) + + # DragToOpenObject + robot = Variable("?robot", _robot_type) + obj = Variable("?obj", _movable_object_type) + parameters = [robot, obj] + preconds = { + LiftedAtom(_Holding, [robot, obj]), + } + add_effs = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_Open, [obj]), + } + del_effs = { + LiftedAtom(_Holding, [robot, obj]), + } + ignore_effs = {_InHandView, _Reachable, _RobotReadyForSweeping, _Blocking} + yield STRIPSOperator("DragToOpenObject", parameters, preconds, add_effs, + del_effs, ignore_effs) + + # DragToCloseObject + robot = Variable("?robot", _robot_type) + obj = Variable("?obj", _movable_object_type) + parameters = [robot, obj] + preconds = { + LiftedAtom(_Holding, [robot, obj]), + } + add_effs = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_NotOpen, [obj]), + } + del_effs = { + LiftedAtom(_Holding, [robot, obj]), + } + ignore_effs = {_InHandView, _Reachable, _RobotReadyForSweeping, _Blocking} + yield STRIPSOperator("DragToCloseObject", parameters, preconds, add_effs, + del_effs, ignore_effs) # MoveToReadySweep robot = Variable("?robot", _robot_type) @@ -1959,6 +2109,25 @@ def _create_operators() -> Iterator[STRIPSOperator]: yield STRIPSOperator("PickAndDumpTwoFromContainer", parameters, preconds, add_effs, del_effs, ignore_effs) + # WipeTable + robot = Variable("?robot", _robot_type) + sweeper = Variable("?sweeper", _movable_object_type) + surface = Variable("?surface", _immovable_object_type) + parameters = [robot, sweeper, surface] + preconds = { + LiftedAtom(_Holding, [robot, sweeper]), + LiftedAtom(_Reachable, [robot, surface]), + LiftedAtom(_IsSweeper, [sweeper]), + LiftedAtom(_HasFlatTopSurface, [surface]), + } + add_effs = { + LiftedAtom(_SurfaceWiped, [surface]), + } + del_effs = set() + ignore_effs = {_Reachable, _InHandView, _RobotReadyForSweeping} + yield STRIPSOperator("WipeTable", parameters, preconds, add_effs, + del_effs, ignore_effs) + ############################################################################### # Shared Utilities for Dry Run Simulation # @@ -1996,7 +2165,7 @@ def _dry_simulate_move_to_view_hand( robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2028,7 +2197,7 @@ def _dry_simulate_move_to_reach_obj( robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2066,7 +2235,7 @@ def _dry_simulate_pick_from_top( robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2138,7 +2307,7 @@ def _dry_simulate_place_on_top( robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2182,7 +2351,7 @@ def _dry_simulate_drop_inside( robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2229,7 +2398,7 @@ def _dry_simulate_drag(last_obs: _SpotObservation, held_obj: Object, robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2284,7 +2453,7 @@ def _dry_simulate_prepare_container_for_sweeping( robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2362,7 +2531,7 @@ def _dry_simulate_sweep_into_container( robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2388,7 +2557,7 @@ def _dry_simulate_drop_not_placeable_object( robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2410,7 +2579,7 @@ def _dry_simulate_noop(last_obs: _SpotObservation, robot_pos=robot_pose, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs @@ -2439,21 +2608,22 @@ def _dry_simulate_pick_and_dump_container( robot_pos=obs.robot_pos, nonpercept_atoms=nonpercept_atoms, nonpercept_predicates=last_obs.nonpercept_predicates, - ) + object_detections_per_camera={}) return next_obs ############################################################################### -# VLM Generic Test Env # +# VLM No Teleop Test Env # ############################################################################### class SpotMinimalVLMPredicateEnv(SpotRearrangementEnv): """An abstract env that makes it easy to test the VLM-based predicate invention and evaluation pipeline on the real Spot robot. Importantly note that every env that inherits from this doesn't - require a map. Rather, it just works directly without a map. TODO: - see if this assumption is actually tenable, or if we need to remove - it? + require a map. Rather, it just works directly without a map. This + means that we don't get to use all the nice navigation skills that + we have on the robot. To use those, we need to use a variant of the + SpotRearrangementEnv from above. """ def __init__(self, use_gui: bool = True) -> None: #pylint:disable=super-init-not-called @@ -2469,7 +2639,10 @@ def __init__(self, use_gui: bool = True) -> None: #pylint:disable=super-init-no self._last_action: Optional[Action] = None # Create constant objects. self._spot_object = Object("robot", _robot_type) - op_to_name = {o.name: o for o in self._create_operators()} + op_to_name = {o.name: o + for o in self._create_operators() + } | {o.name: o + for o in _create_operators()} self._strips_operators = { op_to_name[o] for o in self.op_names_to_keep() @@ -2707,7 +2880,7 @@ def goal_predicates(self) -> Set[Predicate]: @classmethod def get_name(cls) -> str: - return "spot_vlm_cup_table_env" + return "spot_vlm_simple_cup_table_env" @property def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: @@ -2764,6 +2937,135 @@ def _generate_goal_description(self) -> GoalDescription: return "get the cup onto the table!" +class SimpleTableWipingEnv(SpotMinimalVLMPredicateEnv): + """An environment to test the task of actually clearing objects from a + table and then wiping the table.""" + + @property + def predicates(self) -> Set[Predicate]: + preds = set(p for p in _ALL_PREDICATES | _VLM_PREDICATES if p.name in [ + "Holding", "HandEmpty", "NotHolding", "Inside", "VLMOnTable", + "VLMIn", "CanBeUsedForErasing", "TableClean", "TableWiped", + "TableClear", "VLMOnFloor" + ]) + return preds + + @property + def goal_predicates(self) -> Set[Predicate]: + return self.predicates + + @classmethod + def get_name(cls) -> str: + return "spot_vlm_simple_table_wiping_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + objects = { + Object("clear_plastic_trash_can", _immovable_object_type), + Object("fluffy_toy_duster", _movable_object_type), + Object("apple", _movable_object_type), + Object("childrens_play_table", _table_type), + } + for o in objects: + detection_id = LanguageObjectDetectionID(o.name) + detection_id_to_obj[detection_id] = o + return detection_id_to_obj + + def _create_operators(self) -> Iterator[STRIPSOperator]: + # Pick object to clear table. + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + surface = Variable("?table", _table_type) + parameters = [robot, obj, surface] + preconds: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_VLMOnTable, [obj, surface]), + } + add_effs: Set[LiftedAtom] = { + LiftedAtom(_Holding, [robot, obj]), + LiftedAtom(_TableClear, [surface]) + } + del_effs: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_VLMOnTable, [obj, surface]), + } + ignore_effs: Set[Predicate] = set() + yield STRIPSOperator("TeleopPickToClearTable", parameters, preconds, + add_effs, del_effs, ignore_effs) + + # Picking from the floor. + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + surface = Variable("?table", _table_type) + parameters = [robot, obj] + preconds: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + } + add_effs: Set[LiftedAtom] = { + LiftedAtom(_Holding, [robot, obj]), + } + del_effs: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + } + ignore_effs: Set[Predicate] = set() + yield STRIPSOperator("TeleopPickFromFloor", parameters, preconds, + add_effs, del_effs, ignore_effs) + + # Place object inside + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + surf = Variable("?surf", _immovable_object_type) + parameters = [robot, obj, surf] + preconds = {LiftedAtom(_Holding, [robot, obj])} + add_effs = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_VLMIn, [obj, surf]) + } + del_effs = {LiftedAtom(_Holding, [robot, obj])} + ignore_effs = set() + yield STRIPSOperator("TeleopPlaceInside", parameters, preconds, + add_effs, del_effs, ignore_effs) + + # Wipe surface + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + surf = Variable("?surf", _table_type) + parameters = [robot, obj, surf] + preconds = { + LiftedAtom(_Holding, [robot, obj]), + LiftedAtom(_CanBeUsedForErasing, [obj]), + LiftedAtom(_TableClear, [surf]), + } + add_effs = { + LiftedAtom(_TableWiped, [surf]), + } + del_effs = {} + ignore_effs = set() + yield STRIPSOperator("TeleopWipe", parameters, preconds, add_effs, + del_effs, ignore_effs) + + def op_names_to_keep(self) -> Set[str]: + """Return the names of the operators we want to keep.""" + return { + "TeleopPickToClearTable", + "TeleopPlaceInside", + "TeleopWipe", + "MoveToReachObject", + "MoveToHandViewObject", + "TeleopPickFromFloor", + "PlaceObjectOnTop", + } + + def _generate_goal_description(self) -> GoalDescription: + return "clean up the table!" + + class DustpanSweepingTestEnv(SpotMinimalVLMPredicateEnv): """An environment to test a demo task of sweeping some wrappers into a dustpan.""" @@ -2826,8 +3128,8 @@ def _create_operators(self) -> Iterator[STRIPSOperator]: } del_effs = {LiftedAtom(_Holding, [robot, dustpan])} ignore_effs = set() - yield STRIPSOperator("PlaceNextTo", parameters, preconds, add_effs, - del_effs, ignore_effs) + yield STRIPSOperator("TeleopPlaceNextTo", parameters, preconds, + add_effs, del_effs, ignore_effs) # Pick(robot, broom) robot = Variable("?robot", _robot_type) @@ -2860,8 +3162,8 @@ def _create_operators(self) -> Iterator[STRIPSOperator]: add_effs = {LiftedAtom(_Inside, [mess, dustpan])} del_effs = set() ignore_effs = set() - yield STRIPSOperator("Sweep", parameters, preconds, add_effs, del_effs, - ignore_effs) + yield STRIPSOperator("TeleopSweep", parameters, preconds, add_effs, + del_effs, ignore_effs) # Place(robot, broom) robot = Variable("?robot", _robot_type) @@ -2874,14 +3176,14 @@ def _create_operators(self) -> Iterator[STRIPSOperator]: } del_effs = {LiftedAtom(_Holding, [robot, broom])} ignore_effs = set() - yield STRIPSOperator("PlaceOnFloor", parameters, preconds, add_effs, - del_effs, ignore_effs) + yield STRIPSOperator("TeleopPlaceOnFloor", parameters, preconds, + add_effs, del_effs, ignore_effs) def op_names_to_keep(self) -> Set[str]: """Return the names of the operators we want to keep.""" return { - "TeleopPick1", "PlaceNextTo", "TeleopPick2", "Sweep", - "PlaceOnFloor" + "TeleopPick1", "TeleopPlaceNextTo", "TeleopPick2", "TeleopSweep", + "TeleopPlaceOnFloor" } def _generate_goal_description(self) -> GoalDescription: @@ -2997,7 +3299,7 @@ def _get_dry_task(self, train_or_test: str, robot_pos=robot_pose, nonpercept_atoms=self._get_initial_nonpercept_atoms(), nonpercept_predicates=(self.predicates - self.percept_predicates), - ) + object_detections_per_camera={}) # Finish the task. goal_description = self._generate_goal_description() @@ -3421,7 +3723,7 @@ def _get_dry_task(self, train_or_test: str, robot_pos=robot_pose, nonpercept_atoms=self._get_initial_nonpercept_atoms(), nonpercept_predicates=(self.predicates - self.percept_predicates), - ) + object_detections_per_camera={}) # Finish the task. goal_description = self._generate_goal_description() @@ -3429,13 +3731,16 @@ def _get_dry_task(self, train_or_test: str, ############################################################################### -# Brush Shelf Env # +# Bear Panda Bucket Sweep Env # ############################################################################### -class SpotBrushShelfEnv(SpotRearrangementEnv): - """An environment where a brush needs to be moved from the table into one - of the lower shelves.""" +class SpotBearPandaBucketSweepEnv(SpotRearrangementEnv): + """An environment where brown bear and panda toys need to be swept into + a bucket using a blue toy chair as a sweeper. + + This environment tests sweeping multiple objects while avoiding obstacles. + """ def __init__(self, use_gui: bool = True) -> None: super().__init__(use_gui) @@ -3444,24 +3749,60 @@ def __init__(self, use_gui: bool = True) -> None: op_names_to_keep = { "MoveToReachObject", "MoveToHandViewObject", + "MoveToBodyViewObject", "PickObjectFromTop", "PlaceObjectOnTop", + "DragToUnblockObject", + "DragToBlockObject", + "SweepIntoContainer", + "SweepTwoObjectsIntoContainer", + "PrepareContainerForSweeping", + "PickAndDumpContainer", + "PickAndDumpTwoFromContainer", + "DropNotPlaceableObject", + "MoveToReadySweep", + "PickObjectToDrag", + "DropObjectInside", } self._strips_operators = {op_to_name[o] for o in op_names_to_keep} @classmethod def get_name(cls) -> str: - return "spot_brush_shelf_env" + return "lis_spot_bear_panda_bucket_sweep_env" @property def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + # ### + + # chick_toy = Object("chick_toy", _movable_object_type) + # chick_toy_detection = LanguageObjectDetectionID("yellow chick toy/baby chick stuffed animal") + # detection_id_to_obj[chick_toy_detection] = chick_toy + + # ### + + brown_bear = Object("brown_bear_toy", _movable_object_type) + brown_bear_detection = LanguageObjectDetectionID("brown bear toy") + detection_id_to_obj[brown_bear_detection] = brown_bear + + panda = Object("panda_toy", _movable_object_type) + panda_detection = LanguageObjectDetectionID("panda toy") + detection_id_to_obj[panda_detection] = panda + brush = Object("brush", _movable_object_type) - brush_detection = LanguageObjectDetectionID("yellow brush") + brush_detection = LanguageObjectDetectionID(brush_prompt) detection_id_to_obj[brush_detection] = brush + blue_chair = Object("blue_toy_chair", _movable_object_type) + blue_chair_detection = LanguageObjectDetectionID(blue_toy_chair_prompt) + detection_id_to_obj[blue_chair_detection] = blue_chair + + bucket = Object("bucket", _container_type) + bucket_detection = LanguageObjectDetectionID(bucket_prompt) + detection_id_to_obj[bucket_detection] = bucket + for obj, pose in get_known_immovable_objects().items(): detection_id = KnownStaticObjectDetectionID(obj.name, pose) detection_id_to_obj[detection_id] = obj @@ -3469,47 +3810,184 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: return detection_id_to_obj def _generate_goal_description(self) -> GoalDescription: - return "put the brush in the second shelf" + return "sweep the brown bear toy and panda toy into the bucket" def _get_dry_task(self, train_or_test: str, task_idx: int) -> EnvironmentTask: - raise NotImplementedError("Dry task generation not implemented.") - + del train_or_test, task_idx # randomization coming later -############################################################################### -# Real-World Ball and Cup Sticky Table Env # -############################################################################### + # Create the objects and their initial poses. + objects_in_view: Dict[Object, math_helpers.SE3Pose] = {} -# Specific type for sticky table. -_drafting_table_type = Type( - "drafting_table", - list(_immovable_object_type.feature_names) + - ["sticky-region-x", "sticky-region-y"], _immovable_object_type) + # Make up some poses for the objects, with the toys starting on the + # table, the bucket ready for sweeping, and the blue chair ready to use. + metadata = load_spot_metadata() + static_object_feats = metadata["static-object-features"] + known_immovables = metadata["known-immovable-objects"] + table_height = static_object_feats["wooden_table"]["height"] + table_length = static_object_feats["wooden_table"]["length"] + bucket_height = static_object_feats["bucket"]["height"] + blue_chair_height = static_object_feats["blue_toy_chair"]["height"] + blue_chair_width = static_object_feats["blue_toy_chair"]["width"] + floor_z = known_immovables["floor"]["z"] + table_x = known_immovables["wooden_table"]["x"] + table_y = known_immovables["wooden_table"]["y"] + # Create immovable objects. + for obj, pose in get_known_immovable_objects().items(): + objects_in_view[obj] = pose -class SpotBallAndCupStickyTableEnv(SpotRearrangementEnv): - """A real-world version of the ball and cup sticky table environment.""" + # Create robot pose. + robot_se2 = get_spot_home_pose() + robot_pose = robot_se2.get_closest_se3_transform() - def __init__(self, use_gui: bool = True) -> None: - super().__init__(use_gui) - op_to_name = {o.name: o for o in _create_operators()} - op_names_to_keep = { - "MoveToReachObject", "MoveToHandViewObject", - "MoveToBodyViewObject", "PickObjectFromTop", "PlaceObjectOnTop", - "DropObjectInsideContainerOnTop", "PickAndDumpCup" - } - self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + # Create movable objects. + obj_to_xyz: Dict[Object, Tuple[float, float, float]] = {} - @property - def types(self) -> Set[Type]: - return set(_ALL_TYPES) | set([_drafting_table_type]) + # Brown bear toy. + brown_bear = Object("brown_bear_toy", _movable_object_type) + bear_height = static_object_feats.get("brown_bear_toy", {}).get("height", 0.08) + bear_length = static_object_feats.get("brown_bear_toy", {}).get("length", 0.06) + bear_x = table_x - table_length / 2.25 + bear_length + bear_y = table_y + bear_z = floor_z + table_height + bear_height / 2 + obj_to_xyz[brown_bear] = (bear_x, bear_y, bear_z) + + # Panda toy. + panda = Object("panda_toy", _movable_object_type) + panda_height = static_object_feats.get("panda_toy", {}).get("height", 0.08) + panda_x = bear_x + 0.15 + panda_y = bear_y + panda_z = floor_z + table_height + panda_height / 2 + obj_to_xyz[panda] = (panda_x, panda_y, panda_z) + + # Brush (sweeper). + brush = Object("brush", _movable_object_type) + brush_height = static_object_feats.get("brush", {}).get("height", 0.15) + brush_x = robot_pose.x + 1.0 + brush_y = robot_pose.y - 0.5 + brush_z = floor_z + brush_height / 2 + obj_to_xyz[brush] = (brush_x, brush_y, brush_z) + + # Blue toy chair. + blue_chair = Object("blue_toy_chair", _movable_object_type) + chair_x = table_x + chair_y = table_y + 1.5 * blue_chair_width + chair_z = floor_z + blue_chair_height / 2 + obj_to_xyz[blue_chair] = (chair_x, chair_y, chair_z) + + # Bucket. Positioned next to the table for sweeping. + bucket = Object("bucket", _container_type) + bucket_x = table_x - 0.6 + bucket_y = table_y - 0.15 + bucket_z = floor_z + bucket_height / 2 + obj_to_xyz[bucket] = (bucket_x, bucket_y, bucket_z) - @classmethod - def get_name(cls) -> str: - return "spot_ball_and_cup_sticky_table_env" - @property - def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + for obj, (x, y, z) in obj_to_xyz.items(): + pose = math_helpers.SE3Pose(x, y, z, math_helpers.Quat()) + objects_in_view[obj] = pose + + # Create the initial observation. + init_obs = _SpotObservation( + images={}, + objects_in_view=objects_in_view, + objects_in_hand_view=set(), + objects_in_any_view_except_back=set(), + robot=self._spot_object, + gripper_open_percentage=0.0, + robot_pos=robot_pose, + nonpercept_atoms=self._get_initial_nonpercept_atoms(), + nonpercept_predicates=(self.predicates - self.percept_predicates), + ) + + # Finish the task. + goal_description = self._generate_goal_description() + return EnvironmentTask(init_obs, goal_description) + + +############################################################################### +# Brush Shelf Env # +############################################################################### + + +class SpotBrushShelfEnv(SpotRearrangementEnv): + """An environment where a brush needs to be moved from the table into one + of the lower shelves.""" + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + op_to_name = {o.name: o for o in _create_operators()} + op_names_to_keep = { + "MoveToReachObject", + "MoveToHandViewObject", + "PickObjectFromTop", + "PlaceObjectOnTop", + } + self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + + @classmethod + def get_name(cls) -> str: + return "spot_brush_shelf_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + + brush = Object("brush", _movable_object_type) + brush_detection = LanguageObjectDetectionID("yellow brush") + detection_id_to_obj[brush_detection] = brush + + for obj, pose in get_known_immovable_objects().items(): + detection_id = KnownStaticObjectDetectionID(obj.name, pose) + detection_id_to_obj[detection_id] = obj + + return detection_id_to_obj + + def _generate_goal_description(self) -> GoalDescription: + return "put the brush in the second shelf" + + def _get_dry_task(self, train_or_test: str, + task_idx: int) -> EnvironmentTask: + raise NotImplementedError("Dry task generation not implemented.") + + +############################################################################### +# Real-World Ball and Cup Sticky Table Env # +############################################################################### + +# Specific type for sticky table. +_drafting_table_type = Type( + "drafting_table", + list(_immovable_object_type.feature_names) + + ["sticky-region-x", "sticky-region-y"], _immovable_object_type) + + +class SpotBallAndCupStickyTableEnv(SpotRearrangementEnv): + """A real-world version of the ball and cup sticky table environment.""" + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + op_to_name = {o.name: o for o in _create_operators()} + op_names_to_keep = { + "MoveToReachObject", "MoveToHandViewObject", + "MoveToBodyViewObject", "PickObjectFromTop", "PlaceObjectOnTop", + "DropObjectInsideContainerOnTop", "PickAndDumpCup" + } + self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + + @property + def types(self) -> Set[Type]: + return set(_ALL_TYPES) | set([_drafting_table_type]) + + @classmethod + def get_name(cls) -> str: + return "spot_ball_and_cup_sticky_table_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} @@ -3578,10 +4056,10 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} - red_block = Object("red_block", _movable_object_type) - red_block_detection = LanguageObjectDetectionID( - "red block/orange block/yellow block") - detection_id_to_obj[red_block_detection] = red_block + blue_block = Object("blue_block", _movable_object_type) + blue_block_detection = LanguageObjectDetectionID( + "blue block/blue-ish block/blue-green block") + detection_id_to_obj[blue_block_detection] = blue_block for obj, pose in get_known_immovable_objects().items(): detection_id = KnownStaticObjectDetectionID(obj.name, pose) @@ -3590,7 +4068,750 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: return detection_id_to_obj def _generate_goal_description(self) -> GoalDescription: - return "pick up the red block" + return "pick up the blue block" + + def _get_dry_task(self, train_or_test: str, + task_idx: int) -> EnvironmentTask: + raise NotImplementedError("Dry task generation not implemented.") + + +class LISSpotBlockDrawerEnv(SpotRearrangementEnv): + """An extremely basic environment where a block needs to be picked up and + is specifically used for testing in the LIS Spot room. + + Very simple and mostly just for testing. + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + op_to_name = {o.name: o for o in _create_operators()} + op_names_to_keep = { + "MoveToReachObject", + "MoveToHandViewObject", + "PickObjectToDrag", + "DragToOpenObject", + "DragToCloseObject", + } + self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + + @classmethod + def get_name(cls) -> str: + return "lis_spot_block_drawer_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + + blue_block = Object("blue_block", _movable_object_type) + blue_block_detection = LanguageObjectDetectionID( + "blue block/blue-ish block/blue-green block") + detection_id_to_obj[blue_block_detection] = blue_block + + green_handle = Object("green_handle", _movable_object_type) + green_handle_detection = LanguageObjectDetectionID( + "green duct tape/green handle/green object") + detection_id_to_obj[green_handle_detection] = green_handle + + for obj, pose in get_known_immovable_objects().items(): + detection_id = KnownStaticObjectDetectionID(obj.name, pose) + detection_id_to_obj[detection_id] = obj + + return detection_id_to_obj + + def _generate_goal_description(self) -> GoalDescription: + return "open the drawer" + + def _get_dry_task(self, train_or_test: str, + task_idx: int) -> EnvironmentTask: + raise NotImplementedError("Dry task generation not implemented.") + +class LISSpotCollectEnv(SpotRearrangementEnv): + """An extremely basic environment where a block needs to be picked up and + is specifically used for testing in the LIS Spot room. + + Very simple and mostly just for testing. + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + op_to_name = {o.name: o for o in _create_operators()} + op_names_to_keep = { + "MoveToReachObject", + "MoveToHandViewObject", + "PickObjectToDrag", + "DragToOpenObject", + "DragToCloseObject", + "PickObjectFromTop", + "PlaceObjectOnTop", + "DropObjectInside" + } + self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + + @classmethod + def get_name(cls) -> str: + return "lis_spot_collect_misplaced_items_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + + blue_block = Object("blue_block", _movable_object_type) + blue_block_detection = LanguageObjectDetectionID( + "blue block/blue-ish block/blue-green block") + detection_id_to_obj[blue_block_detection] = blue_block + + green_cup = Object("yellow_cup", _movable_object_type) + green_cup_detection = LanguageObjectDetectionID( + "yellow cup/yellow cylinder") + detection_id_to_obj[green_cup_detection] = green_cup + + toy_plane = Object("toy_plane", _movable_object_type) + toy_plane_detection = LanguageObjectDetectionID( + "toy plane") + detection_id_to_obj[toy_plane_detection] = toy_plane + + cardboard_box = Object("cardboard_box", _container_type) + cardboard_box_detection = LanguageObjectDetectionID( + "cardboard box/brown box") + detection_id_to_obj[cardboard_box_detection] = cardboard_box + + green_handle = Object("green_handle", _movable_object_type) + green_handle_detection = LanguageObjectDetectionID( + "green duct tape/green handle/green object") + detection_id_to_obj[green_handle_detection] = green_handle + + for obj, pose in get_known_immovable_objects().items(): + detection_id = KnownStaticObjectDetectionID(obj.name, pose) + detection_id_to_obj[detection_id] = obj + + return detection_id_to_obj + + def _generate_goal_description(self) -> GoalDescription: + return "collect misplaced items" + + def _get_dry_task(self, train_or_test: str, + task_idx: int) -> EnvironmentTask: + raise NotImplementedError("Dry task generation not implemented.") + + +class LISSpotBallsYellowTableEnv(SpotRearrangementEnv): + """An environment where a tennis ball and red ball need to be placed on + a yellow table. Specifically used for testing in the LIS Spot room. + + Very simple and mostly just for testing. + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + op_to_name = {o.name: o for o in _create_operators()} + op_names_to_keep = { + "MoveToReachObject", + "MoveToHandViewObject", + "PickObjectFromTop", + "PlaceObjectOnTop", + "DropObjectInside" + } + self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + + @classmethod + def get_name(cls) -> str: + return "lis_spot_balls_yellow_table_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + + tennis_ball = Object("tennis_ball", _movable_object_type) + tennis_ball_detection = LanguageObjectDetectionID( + "tennis ball/fuzzy yellow ball") + detection_id_to_obj[tennis_ball_detection] = tennis_ball + + red_ball = Object("red_ball", _movable_object_type) + red_ball_detection = LanguageObjectDetectionID( + "red ball/red sphere") + detection_id_to_obj[red_ball_detection] = red_ball + + yellow_table = Object("yellow_table", _immovable_object_type) + yellow_table_detection = LanguageObjectDetectionID( + "short yellow table/yellow stool/yellow toy table") + detection_id_to_obj[yellow_table_detection] = yellow_table + + for obj, pose in get_known_immovable_objects().items(): + detection_id = KnownStaticObjectDetectionID(obj.name, pose) + detection_id_to_obj[detection_id] = obj + + return detection_id_to_obj + + def _generate_goal_description(self) -> GoalDescription: + return "put the tennis ball and red ball on the yellow table" + + def _get_dry_task(self, train_or_test: str, + task_idx: int) -> EnvironmentTask: + raise NotImplementedError("Dry task generation not implemented.") + + +class LISSpotWipeTableEnv(SpotRearrangementEnv): + """An environment where a sponge is used to wipe the wooden table. + + The sponge starts inside an orange bucket. The robot must dump the sponge + out, pick it up, move to the table, and wipe it. + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + op_to_name = {o.name: o for o in _create_operators()} + op_names_to_keep = { + "MoveToReachObject", + "MoveToHandViewObject", + "PickObjectFromTop", + "PlaceObjectOnTop", + "PickAndDumpContainer", + "DropObjectInside", + "WipeTable", + } + self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + + @classmethod + def get_name(cls) -> str: + return "lis_spot_wipe_table_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + + sponge = Object("sponge", _movable_object_type) + sponge_detection = LanguageObjectDetectionID( + "sponge/blue green sponge/cleaning sponge/scrub sponge/blue green spikey sponge/blue scrub brush/spikey toy") + detection_id_to_obj[sponge_detection] = sponge + + orange_bucket = Object("orange_bucket", _container_type) + orange_bucket_detection = LanguageObjectDetectionID(orange_bucket_prompt) + detection_id_to_obj[orange_bucket_detection] = orange_bucket + + for obj, pose in get_known_immovable_objects().items(): + detection_id = KnownStaticObjectDetectionID(obj.name, pose) + detection_id_to_obj[detection_id] = obj + + return detection_id_to_obj + + def _generate_goal_description(self) -> GoalDescription: + return "wipe the wooden table with the sponge" + + def _actively_construct_initial_object_views( + self) -> Dict[Object, math_helpers.SE3Pose]: + """Override to place sponge inside bucket.""" + obj_to_pose = super()._actively_construct_initial_object_views() + # Force sponge to be inside the bucket + sponge = next((o for o in obj_to_pose if o.name == "sponge"), None) + bucket = next((o for o in obj_to_pose if o.name == "orange_bucket"), None) + if sponge is not None and bucket is not None: + bucket_pose = obj_to_pose[bucket] + # Place sponge at bucket center, near bottom + obj_to_pose[sponge] = math_helpers.SE3Pose( + bucket_pose.x, bucket_pose.y, bucket_pose.z - 0.1, bucket_pose.rot) + return obj_to_pose + + def _get_dry_task(self, train_or_test: str, + task_idx: int) -> EnvironmentTask: + raise NotImplementedError("Dry task generation not implemented.") + + +############################################################################### +# VLM Test Env with Map # +############################################################################### + + +class VLMCupEnv(SpotRearrangementEnv): + """A version of the SimpleVLMCupEnv, but with actual skills that the robot + can execute instead of relying on teleop.""" + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + op_to_name = {o.name: o for o in _create_operators()} + op_names_to_keep = { + "MoveToReachObject", + "MoveToHandViewObject", + "PickObjectFromTop", + } + self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + # We add in a place operator that uses VLMOn instead + # of the typical 'OnTop' predicate. + # PlaceObjectOnTop + robot = Variable("?robot", _robot_type) + held = Variable("?held", _movable_object_type) + surface = Variable("?surface", _immovable_object_type) + parameters = [robot, held, surface] + preconds = { + LiftedAtom(_Holding, [robot, held]), + LiftedAtom(_Reachable, [robot, surface]), + LiftedAtom(_NEq, [held, surface]), + LiftedAtom(_IsPlaceable, [held]), + LiftedAtom(_HasFlatTopSurface, [surface]), + LiftedAtom(_FitsInXY, [held, surface]), + } + add_effs = { + LiftedAtom(_VLMOn, [held, surface]), + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, held]), + } + del_effs = { + LiftedAtom(_Holding, [robot, held]), + } + ignore_effs: Set[Predicate] = set() + self._strips_operators.add( + STRIPSOperator("PlaceObjectOnTop", parameters, preconds, add_effs, + del_effs, ignore_effs)) + + @property + def predicates(self) -> Set[Predicate]: + return set(p for p in _ALL_PREDICATES | _VLM_PREDICATES if p.name in [ + "Holding", "HandEmpty", "NotHolding", "Inside", "VLMOn", + "Reachable", "InHandView", "InView" + ]) + + @property + def goal_predicates(self) -> Set[Predicate]: + return self.predicates + + @classmethod + def get_name(cls) -> str: + return "spot_vlm_cup_table_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + objects = { + Object("yellow_toy_cup", _movable_object_type), + Object("small_cardboard_box_with_black_tape", + _immovable_object_type), + } + for o in objects: + detection_id = LanguageObjectDetectionID(o.name) + detection_id_to_obj[detection_id] = o + + for obj, pose in get_known_immovable_objects().items(): + stat_detection_id = KnownStaticObjectDetectionID(obj.name, pose) + detection_id_to_obj[stat_detection_id] = obj + + return detection_id_to_obj + + def _generate_goal_description(self) -> GoalDescription: + return "get the cup onto the table!" + + def _get_dry_task(self, train_or_test: str, + task_idx: int) -> EnvironmentTask: + raise NotImplementedError("Dry task generation not implemented.") + + +############################################################################### +# Table Wiping Env with Map # +############################################################################### + + +class VLMTableWipingOracleEnv(SpotRearrangementEnv): + """A version of the SimpleTableWipingEnv, but with an actual map and some + skills that the robot can execute instead of relying on teleop. + + NOTE: for now, we mostly have teleop actions, but we intend to + replace them with actual skills in the future. + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + # NOTE: temporary just to make data collection easier. + # Comment out this block below when actually running! + # op_to_name = {o.name: o for o in _create_operators()} + # op_names_to_keep = { + # "MoveToReachObject", + # "MoveToHandViewObject", + # } + # self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + + # NOTE: temporary just to make data collection easier. + # Add teleop versions of MoveToReach and MoveToHandView! + # MoveToReachObject + self._strips_operators = set() + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _base_object_type) + parameters = [robot, obj] + preconds = { + LiftedAtom(_NotBlocked, [obj]), + LiftedAtom(_NotHolding, [robot, obj]), + } + add_effs = {LiftedAtom(_Reachable, [robot, obj])} + del_effs: Set[LiftedAtom] = set() + ignore_effs = { + _Reachable, _InHandView, _InView, _RobotReadyForSweeping + } + self._strips_operators.add( + STRIPSOperator("TeleopMoveToReachObject", parameters, preconds, + add_effs, del_effs, ignore_effs)) + + # MoveToHandViewObject + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + parameters = [robot, obj] + preconds = { + LiftedAtom(_NotBlocked, [obj]), + LiftedAtom(_HandEmpty, [robot]) + } + add_effs = {LiftedAtom(_InHandView, [robot, obj])} + del_effs = set() + ignore_effs = { + _Reachable, _InHandView, _InView, _RobotReadyForSweeping + } + self._strips_operators.add( + STRIPSOperator("TeleopMoveToHandViewObject", parameters, preconds, + add_effs, del_effs, ignore_effs)) + + # We now add in specific operators for the table wiping task that + # are tied to specific teleop actions. + # Pick object to clear table. + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + surface = Variable("?table", _table_type) + parameters = [robot, obj, surface] + preconds: Set[LiftedAtom] = { + LiftedAtom(_InHandView, [robot, obj]), + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_VLMOnTable, [obj, surface]), + } + add_effs: Set[LiftedAtom] = { + LiftedAtom(_Holding, [robot, obj]), + LiftedAtom(_TableClear, [surface]) + } + del_effs: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_VLMOnTable, [obj, surface]), + LiftedAtom(_InHandView, [robot, obj]), + } + ignore_effs: Set[Predicate] = set() + self._strips_operators.add( + STRIPSOperator("TeleopPickToClearTable", parameters, preconds, + add_effs, del_effs, ignore_effs)) + + # Picking from the floor. + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + surface = Variable("?table", _table_type) + parameters = [robot, obj] + preconds: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_InHandView, [robot, obj]), + } + add_effs: Set[LiftedAtom] = { + LiftedAtom(_Holding, [robot, obj]), + } + del_effs: Set[LiftedAtom] = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_InHandView, [robot, obj]), + } + ignore_effs: Set[Predicate] = set() + self._strips_operators.add( + STRIPSOperator("TeleopPickFromFloor", parameters, preconds, + add_effs, del_effs, ignore_effs)) + + # Place object inside + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + surf = Variable("?surf", _immovable_object_type) + parameters = [robot, obj, surf] + preconds = { + LiftedAtom(_Holding, [robot, obj]), + LiftedAtom(_Reachable, [robot, surf]), + } + add_effs = { + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_NotHolding, [robot, obj]), + LiftedAtom(_VLMIn, [obj, surf]) + } + del_effs = {LiftedAtom(_Holding, [robot, obj])} + ignore_effs = set() + self._strips_operators.add( + STRIPSOperator("TeleopPlaceInside", parameters, preconds, add_effs, + del_effs, ignore_effs)) + + # Wipe surface + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + surf = Variable("?surf", _table_type) + parameters = [robot, obj, surf] + preconds = { + LiftedAtom(_Reachable, [robot, surf]), + LiftedAtom(_Holding, [robot, obj]), + LiftedAtom(_CanBeUsedForErasing, [obj]), + LiftedAtom(_TableClear, [surf]), + } + add_effs = { + LiftedAtom(_TableWiped, [surf]), + } + del_effs = {} + ignore_effs = set() + self._strips_operators.add( + STRIPSOperator("TeleopWipe", parameters, preconds, add_effs, + del_effs, ignore_effs)) + + @property + def predicates(self) -> Set[Predicate]: + preds = set(p for p in _ALL_PREDICATES | _VLM_PREDICATES if p.name in [ + "Holding", "HandEmpty", "NotHolding", "Inside", "VLMOnTable", + "VLMIn", "CanBeUsedForErasing", "TableClean", "TableWiped", + "TableClear", "VLMOnFloor", "InHandView", "Reachable" + ]) + return preds + + @property + def goal_predicates(self) -> Set[Predicate]: + return self.predicates + + @classmethod + def get_name(cls) -> str: + return "spot_vlm_table_wiping_oracle_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + objects = { + Object("clear_plastic_trash_can", _immovable_object_type), + Object("fluffy_toy_duster", _movable_object_type), + } + for o in objects: + detection_id = LanguageObjectDetectionID(o.name) + detection_id_to_obj[detection_id] = o + + detection_id_to_obj[LanguageObjectDetectionID( + "coffee_table/surfboard")] = Object("table", _table_type) + detection_id_to_obj[LanguageObjectDetectionID( + "apple/red_ball")] = Object("apple", _movable_object_type) + + for obj, pose in get_known_immovable_objects().items(): + stat_detection_id = KnownStaticObjectDetectionID(obj.name, pose) + detection_id_to_obj[stat_detection_id] = obj + + return detection_id_to_obj + + def _generate_goal_description(self) -> GoalDescription: + return "clean up the table!" + + def _get_dry_task(self, train_or_test: str, + task_idx: int) -> EnvironmentTask: + raise NotImplementedError("Dry task generation not implemented.") + + +############################################################################### +# Table Wiping Env with Invented Predicates and Map # +############################################################################### + + +class VLMTableWipingInventedPredsEnv(SpotRearrangementEnv): + """A version of the SimpleTableWipingEnv, but with an actual map and full + skill implementations. + + Also, this environment uses invented predicates for the table wiping + task (these are manually copied over from invention done on the env + in vlm_envs.py). + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + self._OnFloor = utils.create_vlm_predicate( + "OnFloor", [_movable_object_type], + lambda o: _get_vlm_query_str("IsOnFloor", o)) + self._IsEraser = utils.create_vlm_predicate( + "IsEraser", [_movable_object_type], + lambda o: _get_vlm_query_str("IsEraser", o)) + self._OnTop = utils.create_vlm_predicate( + "OnTop", [_movable_object_type, _table_type], + lambda o: _get_vlm_query_str("IsOn", o)) + self._NoObjectsOnTop = utils.create_vlm_predicate( + "NoObjectsOnTop", [_table_type], + lambda s: _get_vlm_query_str("NoObjectsOnTop", s)) + # self._IsGrumpy = utils.create_vlm_predicate( + # "IsGrumpy", [_trash_can_type], + # lambda o: _get_vlm_query_str("IsGrumpy", o)) + + # Add in Operators. + self._strips_operators = set() + # NSRT-Op0: PickFromTop + x0 = Variable("?x0", _movable_object_type) + x1 = Variable("?x1", _table_type) + x2 = Variable("?x2", _robot_type) + parameters = [x2, x0, x1] + preconds = { + LiftedAtom(_HandEmpty, [x2]), + LiftedAtom(self._OnTop, [x0, x1]), + } + add_effs = { + LiftedAtom(_Holding, [x2, x0]), + LiftedAtom(self._NoObjectsOnTop, [x1]), + } + del_effs = { + LiftedAtom(_HandEmpty, [x2]), + LiftedAtom(self._OnTop, [x0, x1]), + } + ignore_effs = set() + self._strips_operators.add( + STRIPSOperator("MoveAndPickFromTop", parameters, preconds, + add_effs, del_effs, ignore_effs)) + + # NSRT-Op1: PlaceInside + x0 = Variable("?x0", _movable_object_type) + x1 = Variable("?x1", _trash_can_type) + x2 = Variable("?x2", _robot_type) + parameters = [x2, x1, x0] + preconds = { + LiftedAtom(_Holding, [x2, x0]), + } + add_effs = { + LiftedAtom(_HandEmpty, [x2]), + LiftedAtom(_VLMIn, [x0, x1]), + } + del_effs = { + LiftedAtom(_Holding, [x2, x0]), + } + ignore_effs = set() + self._strips_operators.add( + STRIPSOperator("MoveToReachAndDropInside", parameters, preconds, + add_effs, del_effs, ignore_effs)) + + # NSRT-Op2: PickFromFloor + x0 = Variable("?x0", _movable_object_type) + x1 = Variable("?x1", _robot_type) + parameters = [x1, x0] + preconds = { + LiftedAtom(_HandEmpty, [x1]), + LiftedAtom(self._OnFloor, [x0]), + } + add_effs = { + LiftedAtom(_Holding, [x1, x0]), + } + del_effs = { + LiftedAtom(_HandEmpty, [x1]), + LiftedAtom(self._OnFloor, [x0]), + } + ignore_effs = set() + self._strips_operators.add( + STRIPSOperator("MoveAndPickFromFloor", parameters, preconds, + add_effs, del_effs, ignore_effs)) + + # NSRT-Op3: WipeAndContinueHoldingEraser + x0 = Variable("?x0", _table_type) + x1 = Variable("?x1", _movable_object_type) + x2 = Variable("?x2", _robot_type) + parameters = [x2, x0, x1] + preconds = { + # LiftedAtom(self._ColorIsGreen, [x1]), + LiftedAtom(_Holding, [x2, x1]), + LiftedAtom(self._IsEraser, [x1]), + LiftedAtom(self._NoObjectsOnTop, [x0]), + } + add_effs = { + LiftedAtom(_TableWiped, [x0]), + } + del_effs = set() + ignore_effs = set() + self._strips_operators.add( + STRIPSOperator("MoveAndWipeSurfaceAndContinueHoldingEraser", + parameters, preconds, add_effs, del_effs, + ignore_effs)) + + # NSRT-Op4: DumpContentsOntoFloor + x0 = Variable("?x0", _movable_object_type) + x1 = Variable("?x1", _trash_can_type) + x2 = Variable("?x2", _robot_type) + parameters = [x2, x1, x0] + # # HACK: just for testing, change the params + # parameters = [x2, x1] + preconds = { + LiftedAtom(_HandEmpty, [x2]), + LiftedAtom(_VLMIn, [x0, x1]), + } + add_effs = { + LiftedAtom(self._OnFloor, [x0]), + } + del_effs = { + LiftedAtom(_VLMIn, [x0, x1]), + } + ignore_effs = set() + self._strips_operators.add( + STRIPSOperator("DumpContentsOntoFloor", parameters, preconds, + add_effs, del_effs, ignore_effs)) + + @property + def predicates(self) -> Set[Predicate]: + preds = set(p for p in _ALL_PREDICATES | _VLM_PREDICATES if p.name in [ + "Holding", + "HandEmpty", + "VLMIn", + "TableWiped", + "Inside", + "TableClean", + "TableClear", + "CanBeUsedForErasing", + ]) + preds |= { + self._OnFloor, self._IsEraser, + self._OnTop, self._NoObjectsOnTop + } + return preds + + @property + def goal_predicates(self) -> Set[Predicate]: + return self.predicates + + @classmethod + def get_name(cls) -> str: + return "spot_vlm_table_wiping_invented_predicates_env" + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + detection_id_to_obj[LanguageObjectDetectionID( + "bottle/clear_cup/clear_trashcan")] = Object( + "clear_plastic_dustbin", _trash_can_type) + detection_id_to_obj[LanguageObjectDetectionID( + "apple/red_ball")] = Object("apple", _movable_object_type) + detection_id_to_obj[LanguageObjectDetectionID( + "fluffy_toy/flower_arrangement")] = Object( + "fluffy_green_toy_eraser", _movable_object_type) + # detection_id_to_obj[LanguageObjectDetectionID( + # "blue_block")] = Object("blue_block", _movable_object_type) + + detection_id_to_obj[LanguageObjectDetectionID( + "cardboard_box")] = Object("cardboard_box_bin", _trash_can_type) + # detection_id_to_obj[LanguageObjectDetectionID( + # "blue_coffee_cup")] = Object("blue_coffee_cup", + # _movable_object_type) + for obj, pose in get_known_immovable_objects().items(): + stat_detection_id = KnownStaticObjectDetectionID(obj.name, pose) + # if obj.name == "short_round_coffee_table": + # table_obj = Object("short_round_coffee_table", _table_type) + # detection_id_to_obj[stat_detection_id] = table_obj + if obj.name == "child_play_table": + table_obj = Object("child_play_table", _table_type) + detection_id_to_obj[stat_detection_id] = table_obj + else: + detection_id_to_obj[stat_detection_id] = obj + + return detection_id_to_obj + + def _generate_goal_description(self) -> GoalDescription: + return "clean up the table!" def _get_dry_task(self, train_or_test: str, task_idx: int) -> EnvironmentTask: diff --git a/predicators/envs/vlm_envs.py b/predicators/envs/vlm_envs.py index c5964b7fd6..805d0ff8af 100644 --- a/predicators/envs/vlm_envs.py +++ b/predicators/envs/vlm_envs.py @@ -10,7 +10,9 @@ import numpy as np from gym.spaces import Box +from predicators import utils from predicators.envs import BaseEnv +from predicators.envs.spot_env import _get_vlm_query_str from predicators.settings import CFG from predicators.structs import Action, EnvironmentTask, GroundAtom, Object, \ Predicate, State, Task, Type @@ -141,3 +143,595 @@ def get_vlm_debug_atom_strs(self, "teabag_on_plate(teabag, plate)" ]) return [[a] for a in atom_strs] + + +class SpotVLMTableWipingInventionEnv(VLMPredicateEnv): + """An env that is intended to be the same as the + 'spot_vlm_table_wiping_env' defined in spot_env.py, but useful for actual + predicate invention (the env in spot_env.py requires using a spot_wrapper + approach...).""" + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + # Env-specific types. + self._robot_type = Type("robot", [], self._object_type) + self._movable_object_type = Type("movable", [], + self._object_type) + self._immovable_object_type = Type("immovable", [], + self._object_type) + self._table_type = Type("table", [], self._immovable_object_type) + self._trash_can_type = Type("trashcan", [], + self._immovable_object_type) + self._VLMIn = utils.create_vlm_predicate( + "InsideContainer", + [self._movable_object_type, self._trash_can_type], + lambda o: _get_vlm_query_str("InsideContainer", o)) + self._TableWiped = utils.create_vlm_predicate( + "WipedOfMarkerScribbles", [self._table_type], + lambda o: _get_vlm_query_str("WipedOfMarkerScribbles", o)) + + @classmethod + def get_name(cls) -> str: + return "spot_vlm_table_wiping_invention_env" + + @property + def types(self) -> Set[Type]: + return super().types | { + self._robot_type, self._table_type, self._object_type, + self._movable_object_type, self._immovable_object_type, + self._trash_can_type + } + + def _get_tasks(self, num: int, + rng: np.random.Generator) -> List[EnvironmentTask]: + del rng # unused. + spot_obj = Object("spot", self._robot_type) + table_obj = Object("child_play_table", self._table_type) + apple_obj = Object("apple", self._movable_object_type) + green_block_obj = Object("green_block", self._movable_object_type) + orange_block_obj = Object("orange_block", self._movable_object_type) + spam_tin_obj = Object("spam_tin", self._movable_object_type) + trash_can_obj = Object("seethru_plastic_dustbin", self._trash_can_type) + duster_obj = Object("furry_green_eraser", self._movable_object_type) + cup_obj = Object("red_drink_cup", self._movable_object_type) + carboard_recycling_bin = Object("cardboard_recycling_bin", + self._trash_can_type) + + ret_tasks = [] + for i in range(num): + init_state_dict = { + spot_obj: np.array([]), + trash_can_obj: np.array([]), + } + if i in [0, 3]: + init_state_dict.update({ + table_obj: np.array([]), + duster_obj: np.array([]), + apple_obj: np.array([]), + cup_obj: np.array([]), + }) + goal = { + GroundAtom(self._TableWiped, [table_obj]), + } + elif i == 1: + init_state_dict.update({ + table_obj: np.array([]), + duster_obj: np.array([]), + apple_obj: np.array([]), + cup_obj: np.array([]), + }) + goal = { + GroundAtom(self._VLMIn, [apple_obj, trash_can_obj]), + GroundAtom(self._TableWiped, [table_obj]), + } + elif i == 2: + init_state_dict.update({ + table_obj: np.array([]), + duster_obj: np.array([]), + apple_obj: np.array([]), + }) + init_state_dict[green_block_obj] = np.array([]) + goal = { + GroundAtom(self._TableWiped, [table_obj]), + } + elif i == 4: + init_state_dict.update({ + table_obj: np.array([]), + duster_obj: np.array([]), + apple_obj: np.array([]), + }) + init_state_dict[green_block_obj] = np.array([]) + goal = { + GroundAtom(self._VLMIn, [apple_obj, trash_can_obj]), + GroundAtom(self._TableWiped, [table_obj]), + } + elif i == 5: + init_state_dict.update({ + green_block_obj: np.array([]), + carboard_recycling_bin: np.array([]), + }) + goal = { + GroundAtom(self._VLMIn, + [green_block_obj, carboard_recycling_bin]) + } + elif i == 6: + init_state_dict.update({ + orange_block_obj: np.array([]), + carboard_recycling_bin: np.array([]), + }) + goal = { + GroundAtom(self._VLMIn, + [orange_block_obj, carboard_recycling_bin]) + } + elif i == 7: + init_state_dict.update({ + spam_tin_obj: np.array([]), + carboard_recycling_bin: np.array([]), + }) + goal = { + GroundAtom(self._VLMIn, + [spam_tin_obj, carboard_recycling_bin]) + } + else: + raise NotImplementedError( + "Shouldn't be getting here! i = {}".format(i)) + + ret_tasks.append(EnvironmentTask(State(init_state_dict), goal)) + return ret_tasks + + @property + def predicates(self) -> Set[Predicate]: + return {self._VLMIn, self._TableWiped} + + @property + def goal_predicates(self) -> Set[Predicate]: + return {self._VLMIn, self._TableWiped} + + def get_vlm_debug_atom_strs(self, + train_tasks: List[Task]) -> List[List[str]]: + del train_tasks + atom_strs = set([ + "inAir(apple)", "onTable(apple)", "onFloor(furry_green_eraser)", + "canBeUsedForErasing(furry_green_eraser)", + "noObjectsOntopTable(child_play_table)" + ]) + return [[a] for a in atom_strs] + + +class SpotVLMTableWipingHumanInventionEnv(VLMPredicateEnv): + """An env that is the same as the above, except intended for invention from + human demos!""" + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + # Env-specific types. + self._robot_type = Type("robot", [], self._object_type) + self._movable_object_type = Type("movable", [], + self._object_type) + self._immovable_object_type = Type("immovable", [], + self._object_type) + self._table_type = Type("table", [], self._immovable_object_type) + self._trash_can_type = Type("trashcan", [], + self._immovable_object_type) + self._VLMIn = utils.create_vlm_predicate( + "InsideContainer", + [self._movable_object_type, self._trash_can_type], + lambda o: _get_vlm_query_str("InsideContainer", o)) + self._TableWiped = utils.create_vlm_predicate( + "WipedOfMarkerScribbles", [self._table_type], + lambda o: _get_vlm_query_str("WipedOfMarkerScribbles", o)) + + @classmethod + def get_name(cls) -> str: + return "spot_vlm_table_wiping_human_invention_env" + + @property + def types(self) -> Set[Type]: + return super().types | { + self._robot_type, self._table_type, self._object_type, + self._movable_object_type, self._immovable_object_type, + self._trash_can_type + } + + def _get_tasks(self, num: int, + rng: np.random.Generator) -> List[EnvironmentTask]: + del rng # unused. + hand_obj = Object("hand", self._robot_type) + table_obj = Object("child_play_table", self._table_type) + apple_obj = Object("apple", self._movable_object_type) + green_block_obj = Object("green_block", self._movable_object_type) + orange_block_obj = Object("orange_block", self._movable_object_type) + spam_tin_obj = Object("spam_tin", self._movable_object_type) + trash_can_obj = Object("seethru_plastic_dustbin", self._trash_can_type) + duster_obj = Object("furry_green_eraser", self._movable_object_type) + cup_obj = Object("red_drink_cup", self._movable_object_type) + carboard_recycling_bin = Object("cardboard_recycling_bin", + self._trash_can_type) + + ret_tasks = [] + for i in range(num): + init_state_dict = { + hand_obj: np.array([]), + trash_can_obj: np.array([]), + } + if i in [0, 2]: + init_state_dict.update({ + table_obj: np.array([]), + duster_obj: np.array([]), + apple_obj: np.array([]), + cup_obj: np.array([]), + }) + goal = { + GroundAtom(self._TableWiped, [table_obj]), + } + # elif i == 1: + # init_state_dict.update({ + # table_obj: np.array([]), + # duster_obj: np.array([]), + # apple_obj: np.array([]), + # cup_obj: np.array([]), + # }) + # goal = { + # GroundAtom(self._VLMIn, [apple_obj, trash_can_obj]), + # GroundAtom(self._TableWiped, [table_obj]), + # } + elif i == 1: + init_state_dict.update({ + table_obj: np.array([]), + duster_obj: np.array([]), + apple_obj: np.array([]), + }) + init_state_dict[green_block_obj] = np.array([]) + goal = { + GroundAtom(self._TableWiped, [table_obj]), + } + elif i == 3: + init_state_dict.update({ + table_obj: np.array([]), + duster_obj: np.array([]), + apple_obj: np.array([]), + }) + init_state_dict[green_block_obj] = np.array([]) + goal = { + GroundAtom(self._VLMIn, [apple_obj, trash_can_obj]), + GroundAtom(self._TableWiped, [table_obj]), + } + elif i == 4: + init_state_dict.update({ + green_block_obj: np.array([]), + carboard_recycling_bin: np.array([]), + }) + goal = { + GroundAtom(self._VLMIn, + [green_block_obj, carboard_recycling_bin]) + } + # elif i == 5: + # init_state_dict.update({ + # orange_block_obj: np.array([]), + # carboard_recycling_bin: np.array([]), + # }) + # goal = { + # GroundAtom(self._VLMIn, + # [orange_block_obj, carboard_recycling_bin]) + # } + elif i == 5: + init_state_dict.update({ + spam_tin_obj: np.array([]), + carboard_recycling_bin: np.array([]), + }) + goal = { + GroundAtom(self._VLMIn, + [spam_tin_obj, carboard_recycling_bin]) + } + else: + raise NotImplementedError( + "Shouldn't be getting here! i = {}".format(i)) + + ret_tasks.append(EnvironmentTask(State(init_state_dict), goal)) + + return ret_tasks + + @property + def predicates(self) -> Set[Predicate]: + return {self._VLMIn, self._TableWiped} + + @property + def goal_predicates(self) -> Set[Predicate]: + return {self._VLMIn, self._TableWiped} + + def get_vlm_debug_atom_strs(self, + train_tasks: List[Task]) -> List[List[str]]: + del train_tasks + atom_strs = set([ + "inAir(apple)", "onTable(apple)", "onFloor(furry_green_eraser)", + "canBeUsedForErasing(furry_green_eraser)", + "noObjectsOntopTable(child_play_table)" + ]) + return [[a] for a in atom_strs] + + +class LISThesisPickPlaceEnv(VLMPredicateEnv): + """Environment for LIS thesis pick-and-place task with Spot robot. + + This is for predicate invention from VLM demos involving picking + and placing balls on tables. + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + # Env-specific types. + self._robot_type = Type("robot", [], self._object_type) + self._ball_type = Type("ball", [], self._object_type) + self._table_type = Type("table", [], self._object_type) + + # VLM-based goal predicate: OnTable(ball, table) + def _get_vlm_query_str(pred_name: str, objects) -> str: + return pred_name + "(" + ", ".join( + str(obj.name) for obj in objects) + ")" + + self._OnTable = utils.create_vlm_predicate( + "OnTable", + [self._ball_type, self._table_type], + lambda o: _get_vlm_query_str("OnTable", o)) + + @classmethod + def get_name(cls) -> str: + return "lis_thesis_pickplace" + + @property + def types(self) -> Set[Type]: + return super().types | { + self._robot_type, + self._ball_type, + self._table_type, + } + + @property + def predicates(self) -> Set[Predicate]: + return {self._OnTable} + + @property + def goal_predicates(self) -> Set[Predicate]: + return {self._OnTable} + + def _get_tasks(self, num: int, + rng: np.random.Generator) -> List[EnvironmentTask]: + del rng # unused. + # Define objects matching the demo + spot_obj = Object("spot", self._robot_type) + tenis_ball_obj = Object("tenis_ball", self._ball_type) + red_ball_obj = Object("red_ball", self._ball_type) + yellow_table_obj = Object("yellow_table", self._table_type) + + ret_tasks = [] + for _ in range(num): + init_state_dict = { + spot_obj: np.array([]), + tenis_ball_obj: np.array([]), + red_ball_obj: np.array([]), + yellow_table_obj: np.array([]), + } + # Goal: both balls should be on the yellow table + goal = { + GroundAtom(self._OnTable, [tenis_ball_obj, yellow_table_obj]), + GroundAtom(self._OnTable, [red_ball_obj, yellow_table_obj]), + } + ret_tasks.append(EnvironmentTask(State(init_state_dict), goal)) + return ret_tasks + + def get_vlm_debug_atom_strs(self, + train_tasks: List[Task]) -> List[List[str]]: + """Debug atoms that might be relevant for this domain.""" + del train_tasks + atom_strs = set([ + "holding(tenis_ball)", + "holding(red_ball)", + "OnTable(tenis_ball, yellow_table)", + "OnTable(red_ball, yellow_table)", + "robot_at(spot, yellow_table)", + "robot_at(spot, tenis_ball)", + "robot_at(spot, red_ball)", + ]) + return [[a] for a in atom_strs] + + +class LISThesisSweepEnv(VLMPredicateEnv): + """Environment for LIS thesis sweep task with Spot robot. + + Goal: sweep table and put toys in the plastic bin. + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + # Env-specific types. + self._robot_type = Type("robot", [], self._object_type) + self._tool_type = Type("tool", [], self._object_type) + self._surface_type = Type("surface", [], self._object_type) + self._toy_type = Type("toy", [], self._object_type) + self._container_type = Type("container", [], self._object_type) + + # VLM-based goal predicates + def _get_vlm_query_str(pred_name: str, objects) -> str: + return pred_name + "(" + ", ".join( + str(obj.name) for obj in objects) + ")" + + self._InContainer = utils.create_vlm_predicate( + "InContainer", + [self._toy_type, self._container_type], + lambda o: _get_vlm_query_str("InContainer", o)) + + self._TableSwept = utils.create_vlm_predicate( + "TableSwept", + [self._surface_type], + lambda o: _get_vlm_query_str("TableSwept", o)) + + @classmethod + def get_name(cls) -> str: + return "lis_thesis_sweep" + + @property + def types(self) -> Set[Type]: + return super().types | { + self._robot_type, + self._tool_type, + self._surface_type, + self._toy_type, + self._container_type, + } + + @property + def predicates(self) -> Set[Predicate]: + return {self._InContainer, self._TableSwept} + + @property + def goal_predicates(self) -> Set[Predicate]: + return {self._InContainer, self._TableSwept} + + def _get_tasks(self, num: int, + rng: np.random.Generator) -> List[EnvironmentTask]: + del rng # unused. + # Define objects matching the demo + spot_obj = Object("spot", self._robot_type) + squeegee_obj = Object("squeegee", self._tool_type) + wooden_table_obj = Object("wooden_table", self._surface_type) + floor_obj = Object("floor", self._surface_type) + toy_caterpillar_obj = Object("toy_caterpillar", self._toy_type) + toy_elephant_obj = Object("toy_elephant", self._toy_type) + toy_frog_obj = Object("toy_frog", self._toy_type) + plastic_bin_obj = Object("plastic_bin", self._container_type) + + ret_tasks = [] + for _ in range(num): + init_state_dict = { + spot_obj: np.array([]), + squeegee_obj: np.array([]), + wooden_table_obj: np.array([]), + floor_obj: np.array([]), + toy_caterpillar_obj: np.array([]), + toy_elephant_obj: np.array([]), + toy_frog_obj: np.array([]), + plastic_bin_obj: np.array([]), + } + # Goal: all three toys in the plastic bin + goal = { + GroundAtom(self._InContainer, + [toy_caterpillar_obj, plastic_bin_obj]), + GroundAtom(self._InContainer, + [toy_elephant_obj, plastic_bin_obj]), + GroundAtom(self._InContainer, + [toy_frog_obj, plastic_bin_obj]), + } + ret_tasks.append(EnvironmentTask(State(init_state_dict), goal)) + return ret_tasks + + def get_vlm_debug_atom_strs(self, + train_tasks: List[Task]) -> List[List[str]]: + """Debug atoms that might be relevant for this domain.""" + del train_tasks + atom_strs = set([ + "holding(squeegee)", + "holding(toy_caterpillar)", + "holding(toy_elephant)", + "holding(toy_frog)", + "InContainer(toy_caterpillar, plastic_bin)", + "InContainer(toy_elephant, plastic_bin)", + "InContainer(toy_frog, plastic_bin)", + "TableSwept(wooden_table)", + "OnSurface(squeegee, floor)", + "OnSurface(squeegee, wooden_table)", + ]) + return [[a] for a in atom_strs] + + +class LISThesisScrubEnv(VLMPredicateEnv): + """Environment for LIS thesis scrub task with Spot robot. + + Goal: clean table and put scrub_sponge back in orange_bucket. + """ + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + # Env-specific types. + self._robot_type = Type("robot", [], self._object_type) + self._tool_type = Type("tool", [], self._object_type) + self._surface_type = Type("surface", [], self._object_type) + self._container_type = Type("container", [], self._object_type) + self._furniture_type = Type("furniture", [], self._object_type) + + # VLM-based goal predicates + def _get_vlm_query_str(pred_name: str, objects) -> str: + return pred_name + "(" + ", ".join( + str(obj.name) for obj in objects) + ")" + + self._InContainer = utils.create_vlm_predicate( + "InContainer", + [self._tool_type, self._container_type], + lambda o: _get_vlm_query_str("InContainer", o)) + + self._TableClean = utils.create_vlm_predicate( + "TableClean", + [self._surface_type], + lambda o: _get_vlm_query_str("TableClean", o)) + + @classmethod + def get_name(cls) -> str: + return "lis_thesis_scrub" + + @property + def types(self) -> Set[Type]: + return super().types | { + self._robot_type, + self._tool_type, + self._surface_type, + self._container_type, + self._furniture_type, + } + + @property + def predicates(self) -> Set[Predicate]: + return {self._InContainer, self._TableClean} + + @property + def goal_predicates(self) -> Set[Predicate]: + return {self._InContainer, self._TableClean} + + def _get_tasks(self, num: int, + rng: np.random.Generator) -> List[EnvironmentTask]: + del rng # unused. + # Define objects matching the demo + spot_obj = Object("spot", self._robot_type) + blue_chair_obj = Object("blue_chair", self._furniture_type) + wooden_table_obj = Object("wooden_table", self._surface_type) + orange_bucket_obj = Object("orange_bucket", self._container_type) + scrub_sponge_obj = Object("scrub_sponge", self._tool_type) + + ret_tasks = [] + for _ in range(num): + init_state_dict = { + spot_obj: np.array([]), + blue_chair_obj: np.array([]), + wooden_table_obj: np.array([]), + orange_bucket_obj: np.array([]), + scrub_sponge_obj: np.array([]), + } + # Goal: clean table and scrub_sponge in orange_bucket + goal = { + GroundAtom(self._TableClean, [wooden_table_obj]), + GroundAtom(self._InContainer, + [scrub_sponge_obj, orange_bucket_obj]), + } + ret_tasks.append(EnvironmentTask(State(init_state_dict), goal)) + return ret_tasks + + def get_vlm_debug_atom_strs(self, + train_tasks: List[Task]) -> List[List[str]]: + """Debug atoms that might be relevant for this domain.""" + del train_tasks + atom_strs = set([ + "holding(scrub_sponge)", + "InContainer(scrub_sponge, orange_bucket)", + "TableClean(wooden_table)", + "Blocking(blue_chair, wooden_table)", + "TableAccessible(wooden_table)", + ]) + return [[a] for a in atom_strs] \ No newline at end of file diff --git a/predicators/execution_monitoring/expected_atoms_monitor.py b/predicators/execution_monitoring/expected_atoms_monitor.py index 4c3bd1b564..4d9b1f2c36 100644 --- a/predicators/execution_monitoring/expected_atoms_monitor.py +++ b/predicators/execution_monitoring/expected_atoms_monitor.py @@ -40,11 +40,20 @@ def step(self, state: State) -> bool: for a in (next_expected_atoms - next_expected_vlm_atoms) if not a.holds(state) } + # Query the VLM for the expected atom values by leveraging + # utils.abstract(). This is a bit ugly, but it's better than querying + # for the specific atoms we want to check - especially in the Spot env, + # we already have evaluated all the necessary VLM atoms and put them in + # the state. vlm_unsat_atoms = set() if len(next_expected_vlm_atoms) > 0: - vlm_unsat_atoms = next_expected_atoms - ( - utils.query_vlm_for_atom_vals(next_expected_vlm_atoms, - state)) # pragma: no cover + next_expected_vlm_preds = { + atom.predicate + for atom in next_expected_vlm_atoms + } + vlm_atom_vals = utils.abstract(state, next_expected_vlm_preds) + vlm_atoms_that_hold_in_state = next_expected_atoms & vlm_atom_vals + vlm_unsat_atoms = next_expected_vlm_atoms - vlm_atoms_that_hold_in_state unsat_atoms = non_vlm_unsat_atoms | vlm_unsat_atoms if not unsat_atoms: return False diff --git a/predicators/ground_truth_models/lis_thesis_pickplace/__init__.py b/predicators/ground_truth_models/lis_thesis_pickplace/__init__.py new file mode 100644 index 0000000000..7191729ce9 --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_pickplace/__init__.py @@ -0,0 +1,9 @@ +"""Ground-truth models for LIS thesis pick-place environment.""" + +from .nsrts import LISThesisPickPlaceGroundTruthNSRTFactory +from .options import LISThesisPickPlaceGroundTruthOptionFactory + +__all__ = [ + "LISThesisPickPlaceGroundTruthNSRTFactory", + "LISThesisPickPlaceGroundTruthOptionFactory" +] diff --git a/predicators/ground_truth_models/lis_thesis_pickplace/nsrts.py b/predicators/ground_truth_models/lis_thesis_pickplace/nsrts.py new file mode 100644 index 0000000000..c27c38009c --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_pickplace/nsrts.py @@ -0,0 +1,23 @@ +"""Ground-truth NSRTs for the LIS thesis pick-place environment.""" + +from typing import Dict, Set + +from predicators.ground_truth_models import GroundTruthNSRTFactory +from predicators.structs import NSRT, ParameterizedOption, Predicate, Type + + +class LISThesisPickPlaceGroundTruthNSRTFactory(GroundTruthNSRTFactory): + """Ground-truth NSRTs for the LIS thesis pick-place environment.""" + + @classmethod + def get_env_names(cls) -> Set[str]: # pragma: no cover + return {"lis_thesis_pickplace"} + + @staticmethod + def get_nsrts( + env_name: str, types: Dict[str, Type], predicates: Dict[str, + Predicate], + options: Dict[str, + ParameterizedOption]) -> Set[NSRT]: # pragma: no cover + # NSRTs will be learned via predicate invention + return set() diff --git a/predicators/ground_truth_models/lis_thesis_pickplace/options.py b/predicators/ground_truth_models/lis_thesis_pickplace/options.py new file mode 100644 index 0000000000..5b0927a59a --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_pickplace/options.py @@ -0,0 +1,69 @@ +"""Ground-truth options for the LIS thesis pick-place environment.""" + +from typing import Dict, Sequence, Set + +from gym.spaces import Box + +from predicators import utils +from predicators.ground_truth_models import GroundTruthOptionFactory +from predicators.structs import Action, Array, Object, ParameterizedOption, \ + ParameterizedPolicy, Predicate, State, Type + + +class LISThesisPickPlaceGroundTruthOptionFactory(GroundTruthOptionFactory): + """Ground-truth options for the LIS thesis pick-place environment.""" + + @classmethod + def get_env_names(cls) -> Set[str]: + return {"lis_thesis_pickplace"} + + @classmethod + def get_options( + cls, env_name: str, types: Dict[str, + Type], predicates: Dict[str, + Predicate], + action_space: Box) -> Set[ParameterizedOption]: # pragma: no cover + + del env_name, predicates # unused. + + object_type = types["object"] + ball_type = types["ball"] + table_type = types["table"] + + # Move to an object location + Move = utils.SingletonParameterizedOption( + # variables: [object to move to] + # params: [] + "move", + cls._create_dummy_policy(action_space), + types=[object_type]) + + # Pick up a ball + Pick = utils.SingletonParameterizedOption( + # variables: [ball to pick] + # params: [] + "pick", + cls._create_dummy_policy(action_space), + types=[ball_type]) + + # Place object on table + PlaceOn = utils.SingletonParameterizedOption( + # variables: [object to place, table to place on] + # params: [] + "place_on", + cls._create_dummy_policy(action_space), + types=[ball_type, table_type]) + + return {Move, Pick, PlaceOn} + + @classmethod + def _create_dummy_policy( + cls, action_space: Box) -> ParameterizedPolicy: # pragma: no cover + del action_space # unused + + def policy(state: State, memory: Dict, objects: Sequence[Object], + params: Array) -> Action: + del state, memory, objects, params + raise ValueError("Shouldn't be attempting to run this policy!") + + return policy diff --git a/predicators/ground_truth_models/lis_thesis_scrub/__init__.py b/predicators/ground_truth_models/lis_thesis_scrub/__init__.py new file mode 100644 index 0000000000..5e072ffec6 --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_scrub/__init__.py @@ -0,0 +1,9 @@ +"""Ground-truth models for LIS thesis scrub environment.""" + +from .nsrts import LISThesisScrubGroundTruthNSRTFactory +from .options import LISThesisScrubGroundTruthOptionFactory + +__all__ = [ + "LISThesisScrubGroundTruthNSRTFactory", + "LISThesisScrubGroundTruthOptionFactory" +] diff --git a/predicators/ground_truth_models/lis_thesis_scrub/nsrts.py b/predicators/ground_truth_models/lis_thesis_scrub/nsrts.py new file mode 100644 index 0000000000..468b96da85 --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_scrub/nsrts.py @@ -0,0 +1,23 @@ +"""Ground-truth NSRTs for the LIS thesis scrub environment.""" + +from typing import Dict, Set + +from predicators.ground_truth_models import GroundTruthNSRTFactory +from predicators.structs import NSRT, ParameterizedOption, Predicate, Type + + +class LISThesisScrubGroundTruthNSRTFactory(GroundTruthNSRTFactory): + """Ground-truth NSRTs for the LIS thesis scrub environment.""" + + @classmethod + def get_env_names(cls) -> Set[str]: # pragma: no cover + return {"lis_thesis_scrub"} + + @staticmethod + def get_nsrts( + env_name: str, types: Dict[str, Type], predicates: Dict[str, + Predicate], + options: Dict[str, + ParameterizedOption]) -> Set[NSRT]: # pragma: no cover + # NSRTs will be learned via predicate invention + return set() diff --git a/predicators/ground_truth_models/lis_thesis_scrub/options.py b/predicators/ground_truth_models/lis_thesis_scrub/options.py new file mode 100644 index 0000000000..784cc06705 --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_scrub/options.py @@ -0,0 +1,89 @@ +"""Ground-truth options for the LIS thesis scrub environment.""" + +from typing import Dict, Sequence, Set + +from gym.spaces import Box + +from predicators import utils +from predicators.ground_truth_models import GroundTruthOptionFactory +from predicators.structs import Action, Array, Object, ParameterizedOption, \ + ParameterizedPolicy, Predicate, State, Type + + +class LISThesisScrubGroundTruthOptionFactory(GroundTruthOptionFactory): + """Ground-truth options for the LIS thesis scrub environment.""" + + @classmethod + def get_env_names(cls) -> Set[str]: + return {"lis_thesis_scrub"} + + @classmethod + def get_options( + cls, env_name: str, types: Dict[str, + Type], predicates: Dict[str, + Predicate], + action_space: Box) -> Set[ParameterizedOption]: # pragma: no cover + + del env_name, predicates # unused. + + object_type = types["object"] + tool_type = types["tool"] + surface_type = types["surface"] + container_type = types["container"] + furniture_type = types["furniture"] + + # Move to an object location + Move = utils.SingletonParameterizedOption( + "move", + cls._create_dummy_policy(action_space), + types=[object_type]) + + # Pick up an object + Pick = utils.SingletonParameterizedOption( + "pick", + cls._create_dummy_policy(action_space), + types=[tool_type]) + + # Unblock access to surface + Unblock = utils.SingletonParameterizedOption( + "unblock", + cls._create_dummy_policy(action_space), + types=[furniture_type, surface_type]) + + # Block access to surface + Block = utils.SingletonParameterizedOption( + "block", + cls._create_dummy_policy(action_space), + types=[furniture_type, surface_type]) + + # Dump contents from container + Dump = utils.SingletonParameterizedOption( + "dump", + cls._create_dummy_policy(action_space), + types=[container_type, tool_type]) + + # Scrub a surface + Scrub = utils.SingletonParameterizedOption( + "scrub", + cls._create_dummy_policy(action_space), + types=[tool_type, surface_type]) + + # Place object in container + PlaceIn = utils.SingletonParameterizedOption( + "place_in", + cls._create_dummy_policy(action_space), + types=[tool_type, container_type]) + + return {Move, Pick, Unblock, Block, Dump, Scrub, PlaceIn} + + @classmethod + def _create_dummy_policy( + cls, action_space: Box) -> ParameterizedPolicy: # pragma: no cover + del action_space # unused + + def policy(state: State, memory: Dict, objects: Sequence[Object], + params: Array) -> Action: + del state, memory, objects, params + raise ValueError("Shouldn't be attempting to run this policy!") + + return policy diff --git a/predicators/ground_truth_models/lis_thesis_sweep/__init__.py b/predicators/ground_truth_models/lis_thesis_sweep/__init__.py new file mode 100644 index 0000000000..0d49d7933b --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_sweep/__init__.py @@ -0,0 +1,9 @@ +"""Ground-truth models for LIS thesis sweep environment.""" + +from .nsrts import LISThesisSweepGroundTruthNSRTFactory +from .options import LISThesisSweepGroundTruthOptionFactory + +__all__ = [ + "LISThesisSweepGroundTruthNSRTFactory", + "LISThesisSweepGroundTruthOptionFactory" +] diff --git a/predicators/ground_truth_models/lis_thesis_sweep/nsrts.py b/predicators/ground_truth_models/lis_thesis_sweep/nsrts.py new file mode 100644 index 0000000000..53727efdf8 --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_sweep/nsrts.py @@ -0,0 +1,23 @@ +"""Ground-truth NSRTs for the LIS thesis sweep environment.""" + +from typing import Dict, Set + +from predicators.ground_truth_models import GroundTruthNSRTFactory +from predicators.structs import NSRT, ParameterizedOption, Predicate, Type + + +class LISThesisSweepGroundTruthNSRTFactory(GroundTruthNSRTFactory): + """Ground-truth NSRTs for the LIS thesis sweep environment.""" + + @classmethod + def get_env_names(cls) -> Set[str]: # pragma: no cover + return {"lis_thesis_sweep"} + + @staticmethod + def get_nsrts( + env_name: str, types: Dict[str, Type], predicates: Dict[str, + Predicate], + options: Dict[str, + ParameterizedOption]) -> Set[NSRT]: # pragma: no cover + # NSRTs will be learned via predicate invention + return set() diff --git a/predicators/ground_truth_models/lis_thesis_sweep/options.py b/predicators/ground_truth_models/lis_thesis_sweep/options.py new file mode 100644 index 0000000000..746bda4505 --- /dev/null +++ b/predicators/ground_truth_models/lis_thesis_sweep/options.py @@ -0,0 +1,77 @@ +"""Ground-truth options for the LIS thesis sweep environment.""" + +from typing import Dict, Sequence, Set + +from gym.spaces import Box + +from predicators import utils +from predicators.ground_truth_models import GroundTruthOptionFactory +from predicators.structs import Action, Array, Object, ParameterizedOption, \ + ParameterizedPolicy, Predicate, State, Type + + +class LISThesisSweepGroundTruthOptionFactory(GroundTruthOptionFactory): + """Ground-truth options for the LIS thesis sweep environment.""" + + @classmethod + def get_env_names(cls) -> Set[str]: + return {"lis_thesis_sweep"} + + @classmethod + def get_options( + cls, env_name: str, types: Dict[str, + Type], predicates: Dict[str, + Predicate], + action_space: Box) -> Set[ParameterizedOption]: # pragma: no cover + + del env_name, predicates # unused. + + object_type = types["object"] + tool_type = types["tool"] + surface_type = types["surface"] + toy_type = types["toy"] + container_type = types["container"] + + # Move to an object location + Move = utils.SingletonParameterizedOption( + "move", + cls._create_dummy_policy(action_space), + types=[object_type]) + + # Pick up an object + Pick = utils.SingletonParameterizedOption( + "pick", + cls._create_dummy_policy(action_space), + types=[object_type]) + + # Sweep a surface with held tool + Sweep = utils.SingletonParameterizedOption( + "sweep", + cls._create_dummy_policy(action_space), + types=[surface_type]) + + # Place object on surface + PlaceOn = utils.SingletonParameterizedOption( + "place_on", + cls._create_dummy_policy(action_space), + types=[object_type, surface_type]) + + # Place object in container + PlaceIn = utils.SingletonParameterizedOption( + "place_in", + cls._create_dummy_policy(action_space), + types=[toy_type, container_type]) + + return {Move, Pick, Sweep, PlaceOn, PlaceIn} + + @classmethod + def _create_dummy_policy( + cls, action_space: Box) -> ParameterizedPolicy: # pragma: no cover + del action_space # unused + + def policy(state: State, memory: Dict, objects: Sequence[Object], + params: Array) -> Action: + del state, memory, objects, params + raise ValueError("Shouldn't be attempting to run this policy!") + + return policy diff --git a/predicators/ground_truth_models/spot_env/nsrts.py b/predicators/ground_truth_models/spot_env/nsrts.py index 55dfbe571d..bbffb02485 100644 --- a/predicators/ground_truth_models/spot_env/nsrts.py +++ b/predicators/ground_truth_models/spot_env/nsrts.py @@ -7,12 +7,13 @@ from predicators import utils from predicators.envs import get_or_create_env from predicators.envs.spot_env import SpotRearrangementEnv, \ - _get_sweeping_surface_for_container, get_detection_id_for_object + _get_sweeping_surface_for_container, get_detection_id_for_object, \ + get_robot from predicators.ground_truth_models import GroundTruthNSRTFactory from predicators.settings import CFG from predicators.spot_utils.perception.object_detection import \ - get_grasp_pixel, get_last_detected_objects -from predicators.spot_utils.perception.spot_cameras import \ + detect_objects, get_grasp_pixel, get_last_detected_objects +from predicators.spot_utils.perception.spot_cameras import capture_images, \ get_last_captured_images from predicators.spot_utils.utils import get_allowed_map_regions, \ get_collision_geoms_for_nav, load_spot_metadata, object_to_top_down_geom, \ @@ -32,6 +33,7 @@ def _move_offset_sampler(state: State, robot_obj: Object, robot_geom = spot_pose_to_geom2d(spot_pose) convex_hulls = get_allowed_map_regions() collision_geoms = get_collision_geoms_for_nav(state) + try: distance, angle, _ = sample_move_offset_from_target( obj_to_nav_to_pos, @@ -84,6 +86,10 @@ def _move_to_hand_view_object_sampler(state: State, goal: Set[GroundAtom], robot_obj = objs[0] obj_to_nav_to = objs[1] + if obj_to_nav_to.name == "blue_toy_chair": + min_dist = 1.8 + max_dist = 1.9 + min_angle, max_angle = _get_approach_angle_bounds(obj_to_nav_to, state) return _move_offset_sampler(state, robot_obj, obj_to_nav_to, rng, min_dist, @@ -103,6 +109,12 @@ def _move_to_reach_object_sampler(state: State, goal: Set[GroundAtom], robot_obj = objs[0] obj_to_nav_to = objs[1] + if obj_to_nav_to.name == "wooden_table": + # For the table, we want to be a bit farther so we can reach over it. + # import ipdb; ipdb.set_trace() + min_dist = 0.8 + max_dist = 0.9 + min_angle, max_angle = _get_approach_angle_bounds(obj_to_nav_to, state) ret_val = _move_offset_sampler(state, robot_obj, obj_to_nav_to, rng, min_dist, max_dist, min_angle, max_angle) @@ -114,6 +126,13 @@ def _get_approach_angle_bounds(obj: Object, """Helper for move samplers.""" angle_bounds = load_spot_metadata().get("approach_angle_bounds", {}) if obj.name in angle_bounds: + if obj.name == "blue_toy_chair": + # if location of chair is far from the table + # we cant approach it from the side + # 1.45993 0.6244 -0.0938282 + # 0.082429 0.991738 -0.178306 + if state.get(obj, "x") > 1.0: + return [-1.6, -1.53] return angle_bounds[obj.name] # Mega-hack for when the container is next to something with angle bounds, # i.e., it is ready to sweep. @@ -121,6 +140,8 @@ def _get_approach_angle_bounds(obj: Object, if surface is not None and surface.name in angle_bounds: return angle_bounds[surface.name] # Default to all possible approach angles. + if obj.name == 'green_handle': + return (np.pi/2, np.pi/2) return (-np.pi, np.pi) @@ -228,7 +249,21 @@ def _drag_to_unblock_object_sampler(state: State, goal: Set[GroundAtom], objs: Sequence[Object]) -> Array: # Parameters are relative dx, dy, dyaw to move while holding. del state, goal, objs, rng # randomization coming soon - return np.array([0.0, 0.0, np.pi / 1.5]) + return np.array([0.0, 0.0, -np.pi / 2]) + +def _drag_to_open_object_sampler(state: State, goal: Set[GroundAtom], + rng: np.random.Generator, + objs: Sequence[Object]) -> Array: + # Parameters are relative dx, dy, dyaw to move while holding. + del state, goal, objs, rng # randomization coming soon + return np.array([-0.5, 0.0, 0.0]) + +def _drag_to_close_object_sampler(state: State, goal: Set[GroundAtom], + rng: np.random.Generator, + objs: Sequence[Object]) -> Array: + # Parameters are relative dx, dy, dyaw to move while holding. + del state, goal, objs, rng # randomization coming soon + return np.array([0.5, 0.0, 0.0]) def _drag_to_block_object_sampler(state: State, goal: Set[GroundAtom], @@ -236,7 +271,7 @@ def _drag_to_block_object_sampler(state: State, goal: Set[GroundAtom], objs: Sequence[Object]) -> Array: # Parameters are relative dx, dy, dyaw to move while holding. del state, goal, objs, rng # randomization coming soon - return np.array([0.0, 0.0, -np.pi / 1.5]) + return np.array([0.0, 0.0, np.pi / 2]) def _sweep_into_container_sampler(state: State, goal: Set[GroundAtom], @@ -244,6 +279,7 @@ def _sweep_into_container_sampler(state: State, goal: Set[GroundAtom], objs: Sequence[Object]) -> Array: # Parameters are just one number, a velocity. del goal + # TODO # return np.array([2.0]) if CFG.spot_use_perfect_samplers: if CFG.spot_run_dry: if len(objs) == 6: # SweepTwoObjectsIntoContainer @@ -279,6 +315,50 @@ def _prepare_sweeping_sampler(state: State, goal: Set[GroundAtom], return np.array([param_dict["dx"], param_dict["dy"], param_dict["angle"]]) +def _wipe_table_sampler(state: State, goal: Set[GroundAtom], + rng: np.random.Generator, + objs: Sequence[Object]) -> Array: + # Parameters are stroke_dx, stroke_dy, num_strokes, duration. + del state, goal, objs # not used for now + if CFG.spot_use_perfect_samplers: + # Default values for wiping + stroke_dx = 0.0 + stroke_dy = 0.4 + num_strokes = 5 + duration = 1.0 + else: + stroke_dx = rng.uniform(-0.1, 0.1) + stroke_dy = rng.uniform(0.3, 0.5) + num_strokes = rng.integers(3, 8) + duration = rng.uniform(0.8, 1.5) + return np.array([stroke_dx, stroke_dy, num_strokes, duration]) + + +def _move_and_wipe_table_sampler(state: State, goal: Set[GroundAtom], + rng: np.random.Generator, + objs: Sequence[Object]) -> Array: + target_obj = objs[1] + move_sample_params = load_spot_metadata()["wipe_location"][target_obj.name] + # Hardcoded params; probably need to change in the future. + rel_dx = 0.0 + # # Params for child play table: + # rel_dy = 0.55 + # delta_dx = 0.05 + # delta_dy = 0.0 + # num_wipes = 5 + # Params for round coffee table + rel_dy = 0.25 + delta_dx = 0.05 + delta_dy = 0.0 + num_wipes = 4 + duration_per_stroke = 1.0 + output_params = np.array([ + move_sample_params[0], move_sample_params[1], move_sample_params[2], + rel_dx, rel_dy, delta_dx, delta_dy, num_wipes, duration_per_stroke + ]) + return output_params + + class SpotEnvsGroundTruthNSRTFactory(GroundTruthNSRTFactory): """Ground-truth NSRTs for the Spot Env.""" @@ -289,7 +369,12 @@ def get_env_names(cls) -> Set[str]: "spot_cube_env", "spot_soda_floor_env", "spot_soda_table_env", "spot_soda_bucket_env", "spot_soda_chair_env", "spot_main_sweep_env", "spot_ball_and_cup_sticky_table_env", - "spot_brush_shelf_env", "lis_spot_block_floor_env" + "spot_brush_shelf_env", "lis_spot_block_floor_env", "lis_spot_block_drawer_env", + "lis_spot_collect_misplaced_items_env", "lis_spot_balls_yellow_table_env", + "lis_spot_bear_panda_bucket_sweep_env", "lis_spot_wipe_table_env", + "spot_vlm_simple_table_wiping_env", + "spot_vlm_table_wiping_oracle_env", + "spot_vlm_table_wiping_invented_predicates_env" } @staticmethod @@ -315,6 +400,8 @@ def get_nsrts(env_name: str, types: Dict[str, Type], "DropObjectInside": _drop_object_inside_sampler, "DropObjectInsideContainerOnTop": _drop_object_inside_sampler, "DragToUnblockObject": _drag_to_unblock_object_sampler, + "DragToOpenObject": _drag_to_open_object_sampler, + "DragToCloseObject": _drag_to_close_object_sampler, "DragToBlockObject": _drag_to_block_object_sampler, "SweepIntoContainer": _sweep_into_container_sampler, "SweepTwoObjectsIntoContainer": _sweep_into_container_sampler, @@ -326,7 +413,15 @@ def get_nsrts(env_name: str, types: Dict[str, Type], "PlaceNextTo": utils.null_sampler, "TeleopPick2": utils.null_sampler, "Sweep": utils.null_sampler, - "PlaceOnFloor": utils.null_sampler + "PlaceOnFloor": utils.null_sampler, + "DumpContentsOntoFloor": _pick_object_from_top_sampler, + "MoveAndPickFromFloor": _move_to_hand_view_object_sampler, + "MoveAndPickFromTop": _move_to_hand_view_object_sampler, + "MoveToReachAndDropInside": _move_to_reach_object_sampler, + "MoveAndWipeSurfaceAndContinueHoldingEraser": + _move_and_wipe_table_sampler, + "DumpContentsOntoFloor": _move_to_hand_view_object_sampler, + "WipeTable": _wipe_table_sampler, } # If we're doing proper bilevel planning with a simulator, then @@ -337,7 +432,10 @@ def get_nsrts(env_name: str, types: Dict[str, Type], # similarly in the future. for strips_op in env.strips_operators: - sampler = operator_name_to_sampler[strips_op.name] + if "teleop" in strips_op.name.lower(): + sampler = utils.null_sampler + else: + sampler = operator_name_to_sampler[strips_op.name] option = options[strips_op.name] nsrt = strips_op.make_nsrt( option=option, diff --git a/predicators/ground_truth_models/spot_env/options.py b/predicators/ground_truth_models/spot_env/options.py index e5562c7ad3..455c15052b 100644 --- a/predicators/ground_truth_models/spot_env/options.py +++ b/predicators/ground_truth_models/spot_env/options.py @@ -21,11 +21,12 @@ from predicators.ground_truth_models import GroundTruthOptionFactory from predicators.settings import CFG from predicators.spot_utils.perception.object_detection import \ - get_grasp_pixel, get_last_detected_objects + detect_objects, get_grasp_pixel, get_last_detected_objects from predicators.spot_utils.perception.perception_structs import \ RGBDImageWithContext -from predicators.spot_utils.perception.spot_cameras import \ +from predicators.spot_utils.perception.spot_cameras import capture_images, \ get_last_captured_images +from predicators.spot_utils.skills.spot_dump import dump_container from predicators.spot_utils.skills.spot_grasp import grasp_at_pixel, \ simulated_grasp_at_pixel from predicators.spot_utils.skills.spot_hand_move import close_gripper, \ @@ -36,6 +37,7 @@ from predicators.spot_utils.skills.spot_place import place_at_relative_position from predicators.spot_utils.skills.spot_stow_arm import stow_arm from predicators.spot_utils.skills.spot_sweep import sweep +from predicators.spot_utils.skills.spot_wipe_table import wipe_multiple_strokes from predicators.spot_utils.spot_localization import SpotLocalizer from predicators.spot_utils.utils import DEFAULT_HAND_DROP_OBJECT_POSE, \ DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE, DEFAULT_HAND_POST_DUMP_POSE, \ @@ -335,7 +337,8 @@ def _grasp_policy(name: str, memory: Dict, objects: Sequence[Object], params: Array, - do_dump: bool = False) -> Action: + do_dump: bool = False, + do_not_stow: bool = False) -> Action: del memory # not used robot, _, _ = get_robot() @@ -371,7 +374,8 @@ def _grasp_policy(name: str, state.get(target_obj, "width")) do_stow = not do_dump and \ - target_obj_volume < CFG.spot_grasp_stow_volume_threshold + target_obj_volume < CFG.spot_grasp_stow_volume_threshold and \ + not do_not_stow fn = _grasp_at_pixel_and_maybe_stow_or_dump sim_fn = None # NOTE: cannot simulate using this option, so this # shouldn't be called anyways... @@ -440,22 +444,22 @@ def _sweep_objects_into_container_policy(name: str, robot_obj_idx: int, ) * middle_bottom_surface_pose # Now, compute the actual pose the hand should start sweeping from by # clamping it between the surface poses. - start_x = np.clip(middle_bottom_surface_rel_pose.x, mean_x + 0.175, + start_x = np.clip(middle_bottom_surface_rel_pose.x, mean_x + 0.275, upper_left_surface_rel_pose.x) start_y = np.clip(middle_bottom_surface_rel_pose.y, mean_y + 0.41, upper_left_surface_rel_pose.y) # use absolute value so that we don't get messed up by noise in the # perception height estimate. - start_z = 0.14 + start_z = 0.17 pitch = math_helpers.Quat.from_pitch(np.pi / 2) yaw = math_helpers.Quat.from_yaw(np.pi / 4) rot = pitch * yaw - sweep_start_pose = math_helpers.SE3Pose(x=start_x, - y=start_y, + sweep_start_pose = math_helpers.SE3Pose(x=start_x + 1.0, + y=start_y + 0.4, z=start_z, rot=rot) sweep_move_dx = 0.0 - sweep_move_dy = -0.8 + sweep_move_dy = -1.0 sweep_move_dz = 0.0 # Execute the sweep. Note simulation fn and args not implemented yet. @@ -496,9 +500,389 @@ def _pick_and_dump_policy(name: str, robot_obj_idx: int, target_obj_idx: int, def _fn() -> None: for action in actions: - assert isinstance(action.extra_info, (list, tuple)) - _, _, action_fn, action_fn_args, _, _ = action.extra_info - action_fn(*action_fn_args) + if isinstance(action.extra_info, (list, tuple)): + _, _, action_fn, action_fn_args, _, _ = action.extra_info + action_fn(*action_fn_args) + continue + else: + action_fn = action.extra_info.real_world_fn + action_fn_args = action.extra_info.real_world_fn_args + action_fn(*action_fn_args) + continue + + # Note simulation fn and args not implemented yet. + action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, tuple()) + + return utils.create_spot_env_action(action_extra_info) + + +def _move_to_view_and_grasp_policy(name: str, robot_obj_idx: int, + target_obj_idx: int, state: State, + memory: Dict, objects: Sequence[Object], + params: Array) -> Action: + move_action = _move_to_hand_view_object_policy(state, memory, objects, + params[:2]) + + def _fn() -> None: + assert isinstance(move_action.extra_info, SpotActionExtraInfo) + move_action.extra_info.real_world_fn( + *move_action.extra_info.real_world_fn_args) + time.sleep(0.5) # Wait for the hand image to settle + while True: + robot, localizer, lease_client = get_robot() + rgbds = capture_images(robot, localizer, relocalize=True) + pick_obj_id = get_detection_id_for_object(objects[target_obj_idx]) + _, artifacts = detect_objects([pick_obj_id], rgbds) + try: + grasp_pixel_sample, rot_constraint = get_grasp_pixel( + rgbds, artifacts, pick_obj_id, "hand_color_image", + _options_rng) + + break + except ValueError: + logging.info( + "Object not seen in hand camera! Moving slightly...") + prompt = ("Hit 'c' to have the robot do a random movement " + "or take control and move the robot accordingly. " + "Hit the 'Enter' key when you're done!") + user_pref = input(prompt) + import PIL + PIL.Image.fromarray( + rgbds["hand_color_image"].rotated_rgb).save( + "hand_image_failed_detection.png") + assert lease_client is not None + lease_client.take() + if rot_constraint is None: + rot_quat_tuple = (0.0, 0.0, 0.0, 0.0) + else: + rot_quat_tuple = (rot_constraint.w, rot_constraint.x, + rot_constraint.y, rot_constraint.z) + params_tuple = grasp_pixel_sample + rot_quat_tuple + grasp_action = _pick_object_from_top_policy(state, memory, objects, + np.array(params_tuple)) + assert isinstance(grasp_action.extra_info, SpotActionExtraInfo) + grasp_action.extra_info.real_world_fn( + *grasp_action.extra_info.real_world_fn_args) + + # Note simulation fn and args not implemented yet. + action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, + tuple()) + return utils.create_spot_env_action(action_extra_info) + + +def _move_to_reach_and_drop_inside_policy(name: str, state: State, + memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + move_action = _move_to_reach_object_policy(state, memory, objects, + params[:2]) + + def _fn() -> None: + assert isinstance(move_action.extra_info, SpotActionExtraInfo) + move_action.extra_info.real_world_fn( + *move_action.extra_info.real_world_fn_args) + # The input objects are [robot, container, object], but + # drop_object_inside_policy expects [robot, object, container]. + objs_for_drop = [objects[0], objects[2], objects[1]] + place_inside_action = _drop_object_inside_policy(state, + memory, + objs_for_drop, + params=np.array( + [0.0, 0.0, 0.2])) + assert isinstance(place_inside_action.extra_info, SpotActionExtraInfo) + place_inside_action.extra_info.real_world_fn( + *place_inside_action.extra_info.real_world_fn_args) + + # Note simulation fn and args not implemented yet. + action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, + tuple()) + return utils.create_spot_env_action(action_extra_info) + + +def _move_to_reach_and_wipe_surface_policy(name: str, state: State, + memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + + def _fn() -> None: + robot, localizer, _ = get_robot() + target_pose = math_helpers.SE2Pose(params[0], params[1], params[2]) + navigate_to_absolute_pose(robot, localizer, target_pose) + # ####################### + # # NOTE: just for testing -> ask for the eraser! + # # Move the hand to the side. + # hand_side_pose = math_helpers.SE3Pose(x=0.80, + # y=0.0, + # z=0.25, + # rot=math_helpers.Quat.from_yaw( + # -np.pi / 2)) + # move_hand_to_relative_pose(robot, hand_side_pose) + # # Ask for the eraser. + # open_gripper(robot) + # # Press any key, instead of just enter. Useful for remote control. + # msg = "Put the brush in the robot's gripper, then press any key" + # utils.wait_for_any_button_press(msg) + # close_gripper(robot) + # ################### + # NOTE: these parameters hardcoded for a particular child_play_table + # object njk is experimenting with. Please swap out depending on the + # actual object you have + # start_pose = math_helpers.SE3Pose(x=0.8, + # y=-0.35, + # z=-0.08, + # rot=math_helpers.Quat.from_pitch( + # np.pi / 2)) + start_pose = math_helpers.SE3Pose(x=0.8, + y=-0.1, + z=-0.04, + rot=math_helpers.Quat.from_pitch( + np.pi / 2)) + end_pose = math_helpers.SE3Pose(x=0.65, + y=0.0, + z=0.55, + rot=math_helpers.Quat.from_pitch( + np.pi / 2)) + rel_dx, rel_dy, delta_dx, delta_dy, num_wipes, duration_per_stroke = params[ + 3:9] + wipe_multiple_strokes(robot, start_pose, end_pose, + rel_dx, rel_dy, (delta_dx, delta_dy), + int(num_wipes), duration_per_stroke) + + # Note simulation fn and args not implemented yet. + action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, + tuple()) + return utils.create_spot_env_action(action_extra_info) + + +def _move_to_view_and_grasp_and_dump_policy(name: str, robot_obj_idx: int, + target_obj_idx: int, state: State, + memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + move_action = _move_to_hand_view_object_policy(state, memory, objects, + params[:2]) + + def _fn() -> None: + assert isinstance(move_action.extra_info, SpotActionExtraInfo) + move_action.extra_info.real_world_fn( + *move_action.extra_info.real_world_fn_args) + time.sleep(0.5) # Wait for the hand image to settle + # Initiate grasping. + robot, localizer, _ = get_robot() + rgbds = capture_images(robot, localizer, relocalize=True) + pick_obj_id = get_detection_id_for_object(objects[target_obj_idx]) + _, artifacts = detect_objects([pick_obj_id], rgbds) + grasp_pixel_sample, rot_constraint = get_grasp_pixel( + rgbds, artifacts, pick_obj_id, "hand_color_image", _options_rng) + # We definitely don't stow, and we don't dump because we do that + # separately later. + _grasp_at_pixel_and_maybe_stow_or_dump(robot, + rgbds["hand_color_image"], + grasp_pixel_sample, + rot_constraint, + np.pi / 4, + timeout=20.0, + retry_grasp_after_fail=True, + do_stow=False, + do_dump=False) + # Initiate dumping. + dump_container(robot, + dump_y=-0.3, + place_z=-0.3, + place_angle=np.pi / 2.2) + # Move the hand away, close the gripper. + move_hand_to_relative_pose(robot, DEFAULT_HAND_POST_DUMP_POSE) + close_gripper(robot) + # Move back a step or two to see. + move_back_pose = math_helpers.SE2Pose(-0.4, 0.0, 0.0) + navigate_to_relative_pose(robot, move_back_pose) + + # Note simulation fn and args not implemented yet. + action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, tuple()) + + return utils.create_spot_env_action(action_extra_info) + + +def _move_to_view_and_grasp_policy(name: str, robot_obj_idx: int, + target_obj_idx: int, state: State, + memory: Dict, objects: Sequence[Object], + params: Array) -> Action: + move_action = _move_to_hand_view_object_policy(state, memory, objects, + params[:2]) + + def _fn() -> None: + assert isinstance(move_action.extra_info, SpotActionExtraInfo) + move_action.extra_info.real_world_fn( + *move_action.extra_info.real_world_fn_args) + time.sleep(0.5) # Wait for the hand image to settle + while True: + robot, localizer, lease_client = get_robot() + rgbds = capture_images(robot, localizer, relocalize=True) + pick_obj_id = get_detection_id_for_object(objects[target_obj_idx]) + _, artifacts = detect_objects([pick_obj_id], rgbds) + try: + grasp_pixel_sample, rot_constraint = get_grasp_pixel( + rgbds, artifacts, pick_obj_id, "hand_color_image", + _options_rng) + + break + except ValueError: + logging.info( + "Object not seen in hand camera! Moving slightly...") + prompt = ("Hit 'c' to have the robot do a random movement " + "or take control and move the robot accordingly. " + "Hit the 'Enter' key when you're done!") + user_pref = input(prompt) + import PIL + PIL.Image.fromarray( + rgbds["hand_color_image"].rotated_rgb).save( + "hand_image_failed_detection.png") + assert lease_client is not None + lease_client.take() + if rot_constraint is None: + rot_quat_tuple = (0.0, 0.0, 0.0, 0.0) + else: + rot_quat_tuple = (rot_constraint.w, rot_constraint.x, + rot_constraint.y, rot_constraint.z) + params_tuple = grasp_pixel_sample + rot_quat_tuple + grasp_action = _pick_object_from_top_policy(state, memory, objects, + np.array(params_tuple)) + assert isinstance(grasp_action.extra_info, SpotActionExtraInfo) + grasp_action.extra_info.real_world_fn( + *grasp_action.extra_info.real_world_fn_args) + + # Note simulation fn and args not implemented yet. + action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, + tuple()) + return utils.create_spot_env_action(action_extra_info) + + +def _move_to_reach_and_drop_inside_policy(name: str, state: State, + memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + move_action = _move_to_reach_object_policy(state, memory, objects, + params[:2]) + + def _fn() -> None: + assert isinstance(move_action.extra_info, SpotActionExtraInfo) + move_action.extra_info.real_world_fn( + *move_action.extra_info.real_world_fn_args) + # The input objects are [robot, container, object], but + # drop_object_inside_policy expects [robot, object, container]. + objs_for_drop = [objects[0], objects[2], objects[1]] + place_inside_action = _drop_object_inside_policy(state, + memory, + objs_for_drop, + params=np.array( + [0.0, 0.0, 0.2])) + assert isinstance(place_inside_action.extra_info, SpotActionExtraInfo) + place_inside_action.extra_info.real_world_fn( + *place_inside_action.extra_info.real_world_fn_args) + + # Note simulation fn and args not implemented yet. + action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, + tuple()) + return utils.create_spot_env_action(action_extra_info) + + +def _move_to_reach_and_wipe_surface_policy(name: str, state: State, + memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + + def _fn() -> None: + robot, localizer, _ = get_robot() + target_pose = math_helpers.SE2Pose(params[0], params[1], params[2]) + navigate_to_absolute_pose(robot, localizer, target_pose) + # ####################### + # # NOTE: just for testing -> ask for the eraser! + # # Move the hand to the side. + # hand_side_pose = math_helpers.SE3Pose(x=0.80, + # y=0.0, + # z=0.25, + # rot=math_helpers.Quat.from_yaw( + # -np.pi / 2)) + # move_hand_to_relative_pose(robot, hand_side_pose) + # # Ask for the eraser. + # open_gripper(robot) + # # Press any key, instead of just enter. Useful for remote control. + # msg = "Put the brush in the robot's gripper, then press any key" + # utils.wait_for_any_button_press(msg) + # close_gripper(robot) + # ################### + # NOTE: these parameters hardcoded for a particular child_play_table + # object njk is experimenting with. Please swap out depending on the + # actual object you have + # start_pose = math_helpers.SE3Pose(x=0.8, + # y=-0.35, + # z=-0.08, + # rot=math_helpers.Quat.from_pitch( + # np.pi / 2)) + start_pose = math_helpers.SE3Pose(x=0.8, + y=-0.1, + z=-0.04, + rot=math_helpers.Quat.from_pitch( + np.pi / 2)) + end_pose = math_helpers.SE3Pose(x=0.65, + y=0.0, + z=0.55, + rot=math_helpers.Quat.from_pitch( + np.pi / 2)) + rel_dx, rel_dy, delta_dx, delta_dy, num_wipes, duration_per_stroke = params[ + 3:9] + wipe_multiple_strokes(robot, start_pose, end_pose, + rel_dx, rel_dy, (delta_dx, delta_dy), + int(num_wipes), duration_per_stroke) + + # Note simulation fn and args not implemented yet. + action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, + tuple()) + return utils.create_spot_env_action(action_extra_info) + + +def _move_to_view_and_grasp_and_dump_policy(name: str, robot_obj_idx: int, + target_obj_idx: int, state: State, + memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + move_action = _move_to_hand_view_object_policy(state, memory, objects, + params[:2]) + + def _fn() -> None: + assert isinstance(move_action.extra_info, SpotActionExtraInfo) + move_action.extra_info.real_world_fn( + *move_action.extra_info.real_world_fn_args) + time.sleep(0.5) # Wait for the hand image to settle + # Initiate grasping. + robot, localizer, _ = get_robot() + rgbds = capture_images(robot, localizer, relocalize=True) + pick_obj_id = get_detection_id_for_object(objects[target_obj_idx]) + _, artifacts = detect_objects([pick_obj_id], rgbds) + grasp_pixel_sample, rot_constraint = get_grasp_pixel( + rgbds, artifacts, pick_obj_id, "hand_color_image", _options_rng) + # We definitely don't stow, and we don't dump because we do that + # separately later. + _grasp_at_pixel_and_maybe_stow_or_dump(robot, + rgbds["hand_color_image"], + grasp_pixel_sample, + rot_constraint, + np.pi / 4, + timeout=20.0, + retry_grasp_after_fail=True, + do_stow=False, + do_dump=False) + # Initiate dumping. + dump_container(robot, + dump_y=-0.3, + place_z=-0.3, + place_angle=np.pi / 2.2) + # Move the hand away, close the gripper. + move_hand_to_relative_pose(robot, DEFAULT_HAND_POST_DUMP_POSE) + close_gripper(robot) + # Move back a step or two to see. + move_back_pose = math_helpers.SE2Pose(-0.4, 0.0, 0.0) + navigate_to_relative_pose(robot, move_back_pose) # Note simulation fn and args not implemented yet. action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, @@ -595,6 +979,8 @@ def _pick_object_to_drag_policy(state: State, memory: Dict, params: Array) -> Action: name = "PickObjectToDrag" target_obj_idx = 1 + if objects[target_obj_idx ].name == 'green_handle' or 'chair' in objects[target_obj_idx].name: + return _grasp_policy(name, target_obj_idx, state, memory, objects, params, do_not_stow=True) return _grasp_policy(name, target_obj_idx, state, memory, objects, params) @@ -691,21 +1077,15 @@ def _drop_object_inside_policy(state: State, memory: Dict, del memory # not used name = "DropObjectInside" - robot_obj_idx = 0 container_obj_idx = 2 - - robot, _, _ = get_robot() - + robot, localizer, _ = get_robot() + localizer.localize() dx, dy, dz = params - - robot_obj = objects[robot_obj_idx] - robot_pose = utils.get_se3_pose_from_state(state, robot_obj) - + robot_pose = localizer.get_last_robot_pose() container_obj = objects[container_obj_idx] container_pose = utils.get_se3_pose_from_state(state, container_obj) # The dz parameter is with respect to the top of the container. container_half_height = state.get(container_obj, "height") / 2 - container_rel_pose = robot_pose.inverse() * container_pose place_z = container_rel_pose.z + container_half_height + dz place_rel_pos = math_helpers.Vec3(x=container_rel_pose.x + dx, @@ -739,30 +1119,30 @@ def _move_and_drop_object_inside_policy(state: State, memory: Dict, del memory # not used name = "MoveAndDropObjectInside" - robot_obj_idx = 0 + # robot_obj_idx = 0 container_obj_idx = 2 - ontop_surface_obj_idx = 3 + # ontop_surface_obj_idx = 3 robot, localizer, _ = get_robot() dx, dy, dz = params - robot_obj = objects[robot_obj_idx] - robot_pose = utils.get_se3_pose_from_state(state, robot_obj) + # robot_obj = objects[robot_obj_idx] + # robot_pose = utils.get_se3_pose_from_state(state, robot_obj) container_obj = objects[container_obj_idx] container_pose = utils.get_se3_pose_from_state(state, container_obj) - surface_obj = objects[ontop_surface_obj_idx] + # surface_obj = objects[ontop_surface_obj_idx] - # Special case: the robot is already on top of the surface (because it is - # probably the floor). When this happens, just drop the object. - surface_geom = object_to_top_down_geom(surface_obj, state) - if surface_geom.contains_point(robot_pose.x, robot_pose.y): - # Note simulation fn and args not yet implemented. - action_extra_info = SpotActionExtraInfo(name, objects, _drop_and_stow, - (robot, ), None, tuple()) - return utils.create_spot_env_action(action_extra_info) + # # Special case: the robot is already on top of the surface (because it is + # # probably the floor). When this happens, just drop the object. + # surface_geom = object_to_top_down_geom(surface_obj, state) + # if surface_geom.contains_point(robot_pose.x, robot_pose.y): + # # Note simulation fn and args not yet implemented. + # action_extra_info = SpotActionExtraInfo(name, objects, _drop_and_stow, + # (robot, ), None, tuple()) + # return utils.create_spot_env_action(action_extra_info) # The dz parameter is with respect to the top of the container. container_half_height = state.get(container_obj, "height") / 2 @@ -793,6 +1173,35 @@ def _drag_to_unblock_object_policy(state: State, memory: Dict, tuple()) return utils.create_spot_env_action(action_extra_info) +def _drag_to_open_object_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + del state, memory # not used + + name = "DragToOpenObject" + robot, _, _ = get_robot() + dx, dy, dyaw = params + move_rel_pos = math_helpers.SE2Pose(dx, dy, angle=dyaw) + # Note that simulation fn and args not yet implemented. + action_extra_info = SpotActionExtraInfo(name, objects, _drag_and_release, + (robot, move_rel_pos), None, + tuple()) + return utils.create_spot_env_action(action_extra_info) + +def _drag_to_close_object_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + del state, memory # not used + + name = "DragToCloseObject" + robot, _, _ = get_robot() + dx, dy, dyaw = params + move_rel_pos = math_helpers.SE2Pose(dx, dy, angle=dyaw) + # Note that simulation fn and args not yet implemented. + action_extra_info = SpotActionExtraInfo(name, objects, _drag_and_release, + (robot, move_rel_pos), None, + tuple()) + return utils.create_spot_env_action(action_extra_info) def _drag_to_block_object_policy(state: State, memory: Dict, objects: Sequence[Object], @@ -861,7 +1270,7 @@ def _prepare_container_for_sweeping_policy(state: State, memory: Dict, rot = math_helpers.Quat.from_pitch(np.pi / 2) place_rel_pose = math_helpers.SE3Pose(x=0.6, y=0.0, - z=container_z - 0.15, + z=container_z, # TODO - 0.15, rot=rot) # Push towards the target a little bit after placing. @@ -886,7 +1295,7 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict, name = "MoveToReadySweep" # Always approach from the same angle. - yaw = np.pi / 2.0 + yaw = 0.0 #np.pi / 2.0 # Make up new params. distance = 0.8 params = np.array([distance, yaw]) @@ -900,6 +1309,50 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict, state, memory, objects, params) +def _move_and_pick_fatop_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + name = "MoveAndPickFromTop" + robot_obj_idx = 0 + target_obj_idx = 1 + return _move_to_view_and_grasp_policy(name, robot_obj_idx, target_obj_idx, + state, memory, objects, params) + + +def _move_and_pick_ffloor_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + name = "MoveAndPickFromFloor" + robot_obj_idx = 0 + target_obj_idx = 1 + return _move_to_view_and_grasp_policy(name, robot_obj_idx, target_obj_idx, + state, memory, objects, params) + + +def _move_and_drop_inside_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + name = "MoveToReachAndDropInside" + return _move_to_reach_and_drop_inside_policy(name, state, memory, objects, + params) + + +def _move_and_wipe_surface_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + name = "MoveAndWipeSurfaceAndContinueHoldingEraser" + return _move_to_reach_and_wipe_surface_policy(name, state, memory, objects, + params) + + +def _move_and_grasp_and_dump_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + name = "DumpContentsOntoFloor" + return _move_to_view_and_grasp_and_dump_policy(name, 0, 1, state, memory, + objects, params) + + def _create_teleop_policy_with_name( name: str) -> Callable[[State, Dict, Sequence[Object], Array], Action]: @@ -933,6 +1386,55 @@ def _teleop(robot: Robot, lease_client: LeaseClient) -> None: return _teleop_policy +def _wipe_table_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + del memory # not used + + robot, _, _ = get_robot() + name = "WipeTable" + + robot_obj = objects[0] + surface_obj = objects[2] + + robot_pose = utils.get_se3_pose_from_state(state, robot_obj) + surface_pose = utils.get_se3_pose_from_state(state, surface_obj) + surface_height = state.get(surface_obj, "height") + if surface_obj.name == "wooden_table": + surface_height -= 1.0 + + # Compute relative pose for wiping start position + surface_rel_pose = robot_pose.inverse() * surface_pose + + # Extract parameters + stroke_dx, stroke_dy, num_strokes_float, duration = params + num_strokes = max(1, int(num_strokes_float)) + + # Define wipe start pose relative to robot + pitch = math_helpers.Quat.from_pitch(np.pi / 2) + wipe_start_pose = math_helpers.SE3Pose( + x=surface_rel_pose.x, + y=surface_rel_pose.y - 0.2, + z=surface_height + 0.05, + rot=pitch) + + # End look pose after wiping + end_look_pose = math_helpers.SE3Pose( + x=surface_rel_pose.x - 0.1, + y=surface_rel_pose.y, + z=surface_height + 0.3, + rot=math_helpers.Quat.from_pitch(np.pi / 2.5)) + + # Delta between strokes + delta_between_strokes = (0.05, 0.0) + + action_extra_info = SpotActionExtraInfo( + name, objects, wipe_multiple_strokes, + (robot, wipe_start_pose, end_look_pose, stroke_dx, stroke_dy, + delta_between_strokes, num_strokes, duration), None, ()) + return utils.create_spot_env_action(action_extra_info) + + ############################################################################### # Parameterized option factory # ############################################################################### @@ -954,21 +1456,27 @@ def _teleop(robot: Robot, lease_client: LeaseClient) -> None: "PickObjectToDrag": Box(-np.inf, np.inf, (6, )), "PlaceObjectOnTop": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dz "DropObjectInside": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dz + "MoveAndDropObjectInside": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dz "DropObjectInsideContainerOnTop": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dz "DragToUnblockObject": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dyaw + "DragToOpenObject": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dyaw + "DragToCloseObject": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dyaw "DragToBlockObject": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dyaw "SweepIntoContainer": Box(-np.inf, np.inf, (1, )), # velocity "SweepTwoObjectsIntoContainer": Box(-np.inf, np.inf, (1, )), # same "PrepareContainerForSweeping": Box(-np.inf, np.inf, (3, )), # dx, dy, dyaw "DropNotPlaceableObject": Box(0, 1, (0, )), # empty - "MoveToReadySweep": Box(0, 1, (0, )), # empty - "TeleopPick1": Box(0, 1, (0, )), # empty - "PlaceNextTo": Box(0, 1, (0, )), # empty - "TeleopPick2": Box(0, 1, (0, )), # empty - "TeleopPlace1": Box(0, 1, (0, )), # empty - "Sweep": Box(0, 1, (0, )), # empty - "PlaceOnFloor": Box(0, 1, (0, )) # empty + "MoveToReadySweep": Box(0, 1, (0, )), # empty, + "MoveAndPickFromTop": Box(-np.inf, np.inf, (2, )), # rel dist, grasp + "MoveAndPickFromFloor": Box(-np.inf, np.inf, (2, )), # rel dist, grasp + "DumpContentsOntoFloor": Box(-np.inf, np.inf, (6, )), + "MoveAndWipeSurfaceAndContinueHoldingEraser": + Box(-np.inf, np.inf, (9, ) + ), # move_abs_x, move_abs_y, move_abs_yaw, rel dx, dy, number of wipes + "DumpContentsOntoFloor": Box(-np.inf, np.inf, (2, )), # params for moving. + "MoveToReachAndDropInside": Box(-np.inf, np.inf, (2, )), # rel dist, dyaw + "WipeTable": Box(-np.inf, np.inf, (4, )), # stroke_dx, stroke_dy, num_strokes, duration } # NOTE: the policies MUST be unique because they output actions with extra info @@ -984,21 +1492,25 @@ def _teleop(robot: Robot, lease_client: LeaseClient) -> None: "PickAndDumpTwoFromContainer": _pick_and_dump_two_container_policy, "PlaceObjectOnTop": _place_object_on_top_policy, "DropObjectInside": _drop_object_inside_policy, + "MoveAndDropObjectInside": + _move_and_drop_object_inside_policy, # rel dx, dy, dz "DropObjectInsideContainerOnTop": _move_and_drop_object_inside_policy, "DragToUnblockObject": _drag_to_unblock_object_policy, + "DragToOpenObject": _drag_to_open_object_policy, + "DragToCloseObject": _drag_to_close_object_policy, "DragToBlockObject": _drag_to_block_object_policy, "SweepIntoContainer": _sweep_into_container_policy, "SweepTwoObjectsIntoContainer": _sweep_two_objects_into_container_policy, "PrepareContainerForSweeping": _prepare_container_for_sweeping_policy, "DropNotPlaceableObject": _drop_not_placeable_object_policy, "MoveToReadySweep": _move_to_ready_sweep_policy, - "TeleopPick1": _create_teleop_policy_with_name("TeleopPick1"), - "PlaceNextTo": _create_teleop_policy_with_name("PlaceNextTo"), - "TeleopPlace": _create_teleop_policy_with_name("TeleopPlace"), - "TeleopPick2": _create_teleop_policy_with_name("TeleopPick2"), - "TeleopPlace1": _create_teleop_policy_with_name("TeleopPlace1"), - "Sweep": _create_teleop_policy_with_name("Sweep"), - "PlaceOnFloor": _create_teleop_policy_with_name("PlaceOnFloor") + "MoveAndPickFromTop": _move_and_pick_fatop_policy, + "MoveAndPickFromFloor": _move_and_pick_ffloor_policy, + "DumpContentsOntoFloor": _move_and_grasp_and_dump_policy, + "MoveAndWipeSurfaceAndContinueHoldingEraser": + _move_and_wipe_surface_policy, + "MoveToReachAndDropInside": _move_and_drop_inside_policy, + "WipeTable": _wipe_table_policy, } @@ -1021,8 +1533,12 @@ def __init__(self, operator_name: str, types: List[Type]) -> None: 0, 1, (0, )) _OPERATOR_NAME_TO_POLICY[ "PickObjectFromTop"] = _sim_safe_pick_object_from_top_policy - params_space = _OPERATOR_NAME_TO_PARAM_SPACE[operator_name] - policy = _OPERATOR_NAME_TO_POLICY[operator_name] + if "teleop" in operator_name.lower(): + policy = _create_teleop_policy_with_name(operator_name) + params_space = Box(0, 1, (0, )) # null + else: + params_space = _OPERATOR_NAME_TO_PARAM_SPACE[operator_name] + policy = _OPERATOR_NAME_TO_POLICY[operator_name] super().__init__(operator_name, policy, types, params_space) def __reduce__(self) -> Tuple: @@ -1035,17 +1551,19 @@ class SpotEnvsGroundTruthOptionFactory(GroundTruthOptionFactory): @classmethod def get_env_names(cls) -> Set[str]: return { - "spot_vlm_dustpan_test_env", - "spot_vlm_cup_table_env", - "spot_cube_env", - "spot_soda_floor_env", - "spot_soda_table_env", - "spot_soda_bucket_env", - "spot_soda_chair_env", - "spot_main_sweep_env", - "spot_ball_and_cup_sticky_table_env", - "spot_brush_shelf_env", - "lis_spot_block_floor_env", + "spot_vlm_dustpan_test_env", "spot_vlm_cup_table_env", + "spot_cube_env", "spot_soda_floor_env", "spot_soda_table_env", + "spot_soda_bucket_env", "spot_soda_chair_env", + "spot_main_sweep_env", "spot_ball_and_cup_sticky_table_env", + "spot_brush_shelf_env", "lis_spot_block_floor_env", + "spot_vlm_simple_table_wiping_env", + "spot_vlm_table_wiping_oracle_env", + "spot_vlm_table_wiping_invented_predicates_env", + "lis_spot_block_drawer_env", + "lis_spot_collect_misplaced_items_env", + "lis_spot_balls_yellow_table_env", + "lis_spot_bear_panda_bucket_sweep_env", + "lis_spot_wipe_table_env" } @classmethod diff --git a/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/__init__.py b/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/__init__.py new file mode 100644 index 0000000000..a358bf75e2 --- /dev/null +++ b/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/__init__.py @@ -0,0 +1,9 @@ +"""Ground-truth models for ice tea making environment.""" + +from .nsrts import SpotTableWipingInventionGroundTruthNSRTFactory +from .options import SpotTableWipingInventionGroundTruthOptionFactory + +__all__ = [ + "SpotTableWipingInventionGroundTruthNSRTFactory", + "SpotTableWipingInventionGroundTruthOptionFactory" +] diff --git a/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/nsrts.py b/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/nsrts.py new file mode 100644 index 0000000000..e8eccb6d59 --- /dev/null +++ b/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/nsrts.py @@ -0,0 +1,26 @@ +"""Ground-truth NSRTs for the spot_vlm_table_wiping_invention environment.""" + +from typing import Dict, Set + +from predicators.ground_truth_models import GroundTruthNSRTFactory +from predicators.structs import NSRT, ParameterizedOption, Predicate, Type + + +class SpotTableWipingInventionGroundTruthNSRTFactory(GroundTruthNSRTFactory): + """Ground-truth NSRTs for the tea making environment.""" + + @classmethod + def get_env_names(cls) -> Set[str]: # pragma: no cover + return { + "spot_vlm_table_wiping_invention_env", + "spot_vlm_table_wiping_human_invention_env" + } + + @staticmethod + def get_nsrts( + env_name: str, types: Dict[str, Type], predicates: Dict[str, + Predicate], + options: Dict[str, + ParameterizedOption]) -> Set[NSRT]: # pragma: no cover + # For now, there are just no NSRTs + return set() diff --git a/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/options.py b/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/options.py new file mode 100644 index 0000000000..35493cf22a --- /dev/null +++ b/predicators/ground_truth_models/spot_vlm_table_wiping_invention_env/options.py @@ -0,0 +1,83 @@ +"""Ground-truth options for the spot_vlm_table_wiping_invention_env +environment.""" + +from typing import Dict, Sequence, Set + +from gym.spaces import Box + +from predicators import utils +from predicators.ground_truth_models import GroundTruthOptionFactory +from predicators.structs import Action, Array, Object, ParameterizedOption, \ + ParameterizedPolicy, Predicate, State, Type + + +class SpotTableWipingInventionGroundTruthOptionFactory(GroundTruthOptionFactory + ): + """Ground-truth options for the tea making environment.""" + + @classmethod + def get_env_names(cls) -> Set[str]: + return { + "spot_vlm_table_wiping_invention_env", + "spot_vlm_table_wiping_human_invention_env" + } + + @classmethod + def get_options( + cls, env_name: str, types: Dict[str, + Type], predicates: Dict[str, + Predicate], + action_space: Box) -> Set[ParameterizedOption]: # pragma: no cover + + del env_name, predicates # unused. + + robot_type = types["robot"] + movable_type = types["movable"] + immovable_type = types["immovable"] + table_type = types["table"] + trash_can_type = types["trashcan"] + + MoveToHandViewObject = utils.SingletonParameterizedOption( + "MoveToHandViewObject", + cls._create_dummy_policy(action_space), + types=[robot_type, movable_type]) + PickFromTop = utils.SingletonParameterizedOption( + "PickFromTop", + cls._create_dummy_policy(action_space), + types=[robot_type, movable_type, immovable_type]) + MoveToReachObject = utils.SingletonParameterizedOption( + "MoveToReachObject", + cls._create_dummy_policy(action_space), + types=[robot_type, immovable_type]) + PlaceInside = utils.SingletonParameterizedOption( + "PlaceInside", + cls._create_dummy_policy(action_space), + types=[robot_type, movable_type, immovable_type]) + PickFromFloor = utils.SingletonParameterizedOption( + "PickFromFloor", + cls._create_dummy_policy(action_space), + types=[robot_type, movable_type]) + WipeAndContinueHoldingEraser = utils.SingletonParameterizedOption( + "WipeAndContinueHoldingEraser", + cls._create_dummy_policy(action_space), + types=[robot_type, movable_type, table_type]) + DumpContentsOntoFloor = utils.SingletonParameterizedOption( + "DumpContentsOntoFloor", + cls._create_dummy_policy(action_space), + types=[robot_type, trash_can_type]) + return { + MoveToHandViewObject, PickFromTop, MoveToReachObject, PlaceInside, + PickFromFloor, WipeAndContinueHoldingEraser, DumpContentsOntoFloor + } + + @classmethod + def _create_dummy_policy( + cls, action_space: Box) -> ParameterizedPolicy: # pragma: no cover + del action_space # unused + + def policy(state: State, memory: Dict, objects: Sequence[Object], + params: Array) -> Action: + del state, memory, objects, params + raise ValueError("Shouldn't be attempting to run this policy!") + + return policy diff --git a/predicators/nsrt_learning/strips_learning/clustering_learner.py b/predicators/nsrt_learning/strips_learning/clustering_learner.py index 7cbb9c6b6c..e507e74802 100644 --- a/predicators/nsrt_learning/strips_learning/clustering_learner.py +++ b/predicators/nsrt_learning/strips_learning/clustering_learner.py @@ -154,6 +154,9 @@ def _postprocessing_learn_ignore_effects(self, fraction = len(pnad.datastore) / option_to_dataset_size[option] if fraction >= CFG.cluster_and_intersect_min_datastore_fraction: ret_pnads.append(pnad) + # elif "Clear0" in str(pnad): + # print(len(pnad.datastore)) + # import ipdb; ipdb.set_trace() return ret_pnads diff --git a/predicators/perception/spot_perceiver.py b/predicators/perception/spot_perceiver.py index 1a5f9114ca..65e1662438 100644 --- a/predicators/perception/spot_perceiver.py +++ b/predicators/perception/spot_perceiver.py @@ -1,9 +1,10 @@ """A perceiver specific to spot envs.""" +from datetime import datetime import logging import time from pathlib import Path -from typing import Dict, List, Optional, Set +from typing import Dict, List, Optional, Set, Tuple import imageio.v2 as iio import numpy as np @@ -14,20 +15,105 @@ from predicators import utils from predicators.envs import BaseEnv, get_or_create_env +from predicators.envs.vlm_envs import VLMPredicateEnv from predicators.envs.spot_env import HANDEMPTY_GRIPPER_THRESHOLD, \ - SpotCubeEnv, SpotRearrangementEnv, _drafting_table_type, \ - _PartialPerceptionState, _SpotObservation, in_general_view_classifier + LanguageObjectDetectionID, ObjectDetectionID, RGBDImageWithContext, \ + SegmentedBoundingBox, SpotCubeEnv, SpotRearrangementEnv, \ + _drafting_table_type, _PartialPerceptionState, _SpotObservation, \ + in_general_view_classifier from predicators.perception.base_perceiver import BasePerceiver from predicators.settings import CFG from predicators.spot_utils.utils import _container_type, _dustpan_type, \ - _immovable_object_type, _movable_object_type, _robot_type, \ - _wrappers_type, get_allowed_map_regions, load_spot_metadata, \ - object_to_top_down_geom + _immovable_object_type, _movable_object_type, _robot_type, _table_type, \ + _trash_can_type, _wrappers_type, get_allowed_map_regions, \ + load_spot_metadata, object_to_top_down_geom from predicators.structs import Action, DefaultState, EnvironmentTask, \ GoalDescription, GroundAtom, Object, Observation, Predicate, \ SpotActionExtraInfo, State, Task, Video, VLMPredicate, _Option +# Helper functions. +CAMERA_NAME_TO_ANNOTATION = { + 'hand_color_image': "Hand Camera Image", + 'back_fisheye_image': "Back Camera Image", + 'frontleft_fisheye_image': "Front Left Camera Image", + 'frontright_fisheye_image': "Front Right Camera Image", + 'left_fisheye_image': "Left Camera Image", + 'right_fisheye_image': "Right Camera Image" +} + +def annotate_imgs_with_detections( + img_objects: Dict[str, RGBDImageWithContext], + object_detections_per_camera: Dict[str, List[Tuple[ObjectDetectionID, + SegmentedBoundingBox]]] +) -> List[PIL.Image.Image]: + """Annotate images via editing the pixels directly to include object + detection bounding boxes and camera names.""" + img_names = [v.camera_name for _, v in img_objects.items()] + imgs = [v.rotated_rgb for _, v in img_objects.items()] + pil_imgs = [PIL.Image.fromarray(img) for img in imgs] # type: ignore + # Annotate images with detected objects (names + bounding box) + # and camera name. + for i, camera_name in enumerate(img_names): + draw = ImageDraw.Draw(pil_imgs[i]) + # Annotate with camera name. + font = utils.get_scaled_default_font(draw, 4) + _ = utils.add_text_to_draw_img(draw, (0, 0), + CAMERA_NAME_TO_ANNOTATION[camera_name], + font) + # TODO: just commenting out for now to see if this helps labelling. + # # Annotate with object detections. + # detections = object_detections_per_camera[camera_name] + # for obj_id, seg_bb in detections: + # if isinstance(obj_id, LanguageObjectDetectionID): + # x0, y0, x1, y1 = seg_bb.bounding_box + # x0, x1 = sorted([x0, x1]) + # y0, y1 = sorted([y0, y1]) + # draw.rectangle([(x0, y0), (x1, y1)], outline='green', width=2) + # text = f"{obj_id.language_id}" + # font = utils.get_scaled_default_font(draw, 3) + # text_mask = font.getmask(text) # type: ignore + # text_width, text_height = text_mask.size + # text_bbox = [(x0, y0 - 1.5 * text_height), + # (x0 + text_width + 1, y0)] + # draw.rectangle(text_bbox, fill='green') + # draw.text((x0 + 1, y0 - 1.5 * text_height), + # text, + # fill='white', + # font=font) + annotated_imgs = list(pil_imgs) + return annotated_imgs + + +def save_annotated_imgs_for_vlm_demo(annotated_imgs: List[PIL.Image.Image], + save_dir: Path) -> None: + """Save annotated images useful images as part of creating demonstrations + for learning from teleop'ed Spot trajectories.""" + # If `save_dir` doesn't exist, create it. + save_dir.mkdir(parents=True, exist_ok=True) + # Collect all the names of folders within `save_dir`. + subfolders = [f for f in save_dir.iterdir() if f.is_dir()] + # Find the highest int that's a subfolder of `save_dir`. + prev_timestep = -1 + for folder in subfolders: + try: + folder_int = int(folder.name) + prev_timestep = max(prev_timestep, folder_int) + except ValueError: + # Skip folders that are not integers. + continue + # Add 1 to `prev_timestep` to get `curr_timestep`. + curr_timestep = prev_timestep + 1 + curr_timestep_dir = save_dir / str(curr_timestep) + # Create the new folder for `curr_timestep`. + curr_timestep_dir.mkdir(parents=True, exist_ok=True) + # Save all the images as jpg files under the new folder. + for i, img in enumerate(annotated_imgs): + img_path = curr_timestep_dir / f"image_{i}.jpg" + img.save(img_path, format="JPEG") + + +# Main perceiver classes. class SpotPerceiver(BasePerceiver): """A perceiver specific to spot envs.""" @@ -57,6 +143,13 @@ def __init__(self) -> None: # Load static, hard-coded features of objects, like their shapes. meta = load_spot_metadata() self._static_object_features = meta.get("static-object-features", {}) + # Histories and other artefacts (for VLM labelling). + self._curr_state: Optional[State] = DefaultState + self._curr_state.simulator_state = {} + self._curr_annotated_imgs: List[PIL.Image.Image] = [] + self._state_history: List[State] = [] + self._executed_skill_history: List[Optional[_Option]] = [] + self._vlm_label_history: List[str] = [] @classmethod def get_name(cls) -> str: @@ -67,24 +160,56 @@ def reset(self, env_task: EnvironmentTask) -> Task: if self._waiting_for_observation or CFG.spot_run_dry: self._waiting_for_observation = True self._curr_env = get_or_create_env(CFG.env) - assert isinstance(self._curr_env, SpotRearrangementEnv) - self._known_object_poses = {} - self._objects_in_view = set() - self._objects_in_hand_view = set() - self._objects_in_any_view_except_back = set() - self._robot = None - self._nonpercept_atoms = set() - self._nonpercept_predicates = set() - self._percept_predicates = self._curr_env.percept_predicates - self._held_object = None - self._gripper_open_percentage = 0.0 - self._robot_pos = math_helpers.SE3Pose(0, 0, 0, - math_helpers.Quat()) - self._lost_objects = set() - self._container_to_contained_objects = {} + if isinstance(self._curr_env, SpotRearrangementEnv): + self._known_object_poses = {} + self._objects_in_view = set() + self._objects_in_hand_view = set() + self._objects_in_any_view_except_back = set() + self._robot = None + self._nonpercept_atoms = set() + self._nonpercept_predicates = set() + self._percept_predicates = self._curr_env.percept_predicates + self._held_object = None + self._gripper_open_percentage = 0.0 + self._robot_pos = math_helpers.SE3Pose(0, 0, 0, + math_helpers.Quat()) + self._lost_objects = set() + self._container_to_contained_objects = {} + elif isinstance(self._curr_env, VLMPredicateEnv): + self._known_object_poses = {} + self._objects_in_view = set() + self._objects_in_hand_view = set() + self._objects_in_any_view_except_back = set() + self._robot = None + self._nonpercept_atoms = set() + self._nonpercept_predicates = set() + self._percept_predicates = self._curr_env.predicates + self._held_object = None + self._gripper_open_percentage = 0.0 + self._robot_pos = math_helpers.SE3Pose(0, 0, 0, + math_helpers.Quat()) + self._lost_objects = set() + self._container_to_contained_objects = {} + else: + raise NotImplementedError(f"Don't know how to reset perceiver for env "f"type {type(self._curr_env)}.") + + self._prev_action = None # already processed at the end of the cycle init_state = self._create_state() - goal = self._create_goal(init_state, env_task.goal_description) + if isinstance(self._curr_env, VLMPredicateEnv): + import ipdb; ipdb.set_trace() + goal = self.get_goal_atoms_from_description(env_task.goal_description) + else: + goal = self._create_goal(init_state, env_task.goal_description) + + # Reset run-specific things. + self._curr_state = DefaultState + self._curr_state.simulator_state = {} + self._state_history = [] + self._executed_skill_history = [] + self._vlm_label_history = [] + self._prev_action = None + return Task(init_state, goal) def update_perceiver_with_action(self, action: Action) -> None: @@ -96,6 +221,11 @@ def update_perceiver_with_action(self, action: Action) -> None: def step(self, observation: Observation) -> State: self._update_state_from_observation(observation) + # If we're trying to record VLM demos, then save the images. + if len(CFG.spot_vlm_teleop_demo_folderpath) > 0: + save_annotated_imgs_for_vlm_demo( + self._curr_annotated_imgs, + Path(CFG.spot_vlm_teleop_demo_folderpath)) # Update the curr held item when applicable. assert self._curr_env is not None if self._prev_action is not None: @@ -133,6 +263,9 @@ def step(self, observation: Observation) -> State: # Check if the item we just placed is in view. It needs to # be in view to assess whether it was placed correctly. robot, obj = objects[:2] + if controller_name == "MoveToReachAndDropInside": + # The object is the 3rd argument in this case. + obj = objects[2] state = self._create_state() is_in_view = in_general_view_classifier(state, [robot, obj]) if not is_in_view: @@ -143,7 +276,7 @@ def step(self, observation: Observation) -> State: for n in ["sweepintocontainer", "sweeptwoobjects"]): robot = objects[0] state = self._create_state() - if controller_name.lower() == "sweepintocontainer": + if controller_name.lower() == "sweepintocontsainer": objs = {objects[2]} else: assert controller_name.lower().startswith("sweeptwoobject") @@ -166,7 +299,6 @@ def step(self, observation: Observation) -> State: logging.info("[Perceiver] An object was lost: " f"{prev_held_object} was lost!") self._lost_objects.add(prev_held_object) - return self._create_state() def _update_state_from_observation(self, observation: Observation) -> None: @@ -202,10 +334,13 @@ def _update_state_from_observation(self, observation: Observation) -> None: self._robot_pos = observation.robot_pos for obj in observation.objects_in_view: self._lost_objects.discard(obj) + self._curr_annotated_imgs = annotate_imgs_with_detections( + observation.images, observation.object_detections_per_camera) def _create_state(self) -> State: if self._waiting_for_observation: return DefaultState + assert self._curr_state is not None # Build the continuous part of the state. assert self._robot is not None state_dict = { @@ -284,10 +419,77 @@ def _create_state(self) -> State: # logging.info("Simulator state:") # logging.info(simulator_state) + # Add the images and histories into the simulator_state. + simulator_state["images"] = self._curr_annotated_imgs + # At the first timestep, these histories will be empty due to + # self.reset(). But at every timestep that isn't the first one, + # they will be non-empty. + simulator_state["state_history"] = list(self._state_history) + # We do this here so the call to `utils.abstract()` a few lines later + # has the skill that was just run. + executed_skill = None + + if self._prev_action is not None: + assert self._prev_action.extra_info is not None + if self._prev_action.extra_info.action_name == "done": + # Just return the default state + return DefaultState + if self._prev_action.has_option(): + executed_skill = self._prev_action.get_option() + self._executed_skill_history.append( + executed_skill) # None in first timestep. + simulator_state["skill_history"] = list(self._executed_skill_history) + simulator_state["vlm_label_history"] = list(self._vlm_label_history) + + # Add to histories. + # A bit of extra work is required to build the VLM label history. + # We want to keep `utils.abstract()` as straightforward as possible, + # so we'll "rebuild" the VLM labels from the abstract state + # returned by `utils.abstract()`. And since we call this function, + # we might as well store the abstract state as a part of the simulator + # state so that we don't need to recompute it later in the approach or + # in planning. + assert self._curr_env is not None + preds = self._curr_env.predicates + # Create a _PartialPerceptionState before abstracting because some + # predicate classifiers (e.g., _surface_wiped_classifier) require it. + state_copy = _PartialPerceptionState(percept_state.data, + simulator_state=simulator_state) + abstract_state = utils.abstract(state_copy, preds) + simulator_state["abstract_state"] = abstract_state + print(f"abstract_state: {abstract_state}") + # Compute all the VLM atoms. `utils.abstract()` only returns the ones + # that are True. The remaining ones are the ones that are False. + vlm_preds = set(pred for pred in preds + if isinstance(pred, VLMPredicate)) + vlm_atoms = set() + for pred in vlm_preds: + for choice in utils.get_object_combinations( + list(state_copy), pred.types): + vlm_atoms.add(GroundAtom(pred, choice)) + vlm_atoms_list = sorted(vlm_atoms) + reconstructed_all_vlm_responses = [] + for atom in vlm_atoms_list: + if atom in abstract_state: + truth_value = 'True' + else: + truth_value = 'False' + atom_label = f"* {atom.get_vlm_query_str()}: {truth_value}" + reconstructed_all_vlm_responses.append(atom_label) + str_vlm_response = '\n'.join(reconstructed_all_vlm_responses) + self._vlm_label_history.append(str_vlm_response) + # Now finish the state. state = _PartialPerceptionState(percept_state.data, simulator_state=simulator_state) + + # Save state for debugging + now = datetime.now() + with open(CFG.spot_perception_outdir + f"/0_{now.strftime('%Y%m%d_%H%M%S')}_latest_perceived_state.txt", "w") as f: + f.write(state.pretty_str()) + self._curr_state = state + self._state_history.append(self._curr_state.copy()) return state def _create_goal(self, state: State, @@ -455,6 +657,28 @@ def _create_goal(self, state: State, return { GroundAtom(On, [bucket, shelf]), } + if goal_description == "sweep the brown bear toy and panda toy into the bucket": + brown_bear = Object("brown_bear_toy", _movable_object_type) + panda = Object("panda_toy", _movable_object_type) + chick_toy = Object("chick_toy", _movable_object_type) + bucket = Object("bucket", _container_type) + Inside = pred_name_to_pred["Inside"] + # table = Object("wooden_table", _immovable_object_type) + # On = pred_name_to_pred["On"] + wooden_table = Object("wooden_table", _immovable_object_type) + NotBlocked = pred_name_to_pred["NotBlocked"] + blue_toy_chair = Object("blue_toy_chair", _movable_object_type) + Blocking = pred_name_to_pred["Blocking"] + NotInsideAnyContainer = pred_name_to_pred["NotInsideAnyContainer"] + return { + GroundAtom(Inside, [brown_bear, bucket]), + GroundAtom(Inside, [panda, bucket]), + # GroundAtom(Inside, [chick_toy, bucket]), + # GroundAtom(NotBlocked, [wooden_table]), + # GroundAtom(Blocking, [blue_toy_chair, wooden_table]), + #GroundAtom(NotInsideAnyContainer, [brown_bear]), + #GroundAtom(NotInsideAnyContainer, [panda]) + } if goal_description == "pick up the brush": robot = Object("robot", _robot_type) brush = Object("brush", _movable_object_type) @@ -467,6 +691,54 @@ def _create_goal(self, state: State, block = Object("red_block", _movable_object_type) Holding = pred_name_to_pred["Holding"] return {GroundAtom(Holding, [robot, block])} + if goal_description == "pick up the blue block": + robot = Object("robot", _robot_type) + block = Object("blue_block", _movable_object_type) + Holding = pred_name_to_pred["Holding"] + return {GroundAtom(Holding, [robot, block])} + if goal_description == "open the drawer": + robot = Object("robot", _robot_type) + handle = Object("green_handle", _movable_object_type) + Open = pred_name_to_pred["Open"] + return {GroundAtom(Open, [handle])} + if goal_description == "close the drawer": + robot = Object("robot", _robot_type) + handle = Object("green_handle", _movable_object_type) + NotOpen = pred_name_to_pred["NotOpen"] + return {GroundAtom(NotOpen, [handle])} + if goal_description == "collect misplaced items": + robot = Object("robot", _robot_type) + handle = Object("green_handle", _movable_object_type) + blue_block = Object("blue_block", _movable_object_type) + yellow_cup = Object("yellow_cup", _movable_object_type) + toy_plane = Object("toy_plane", _movable_object_type) + cardboard_box = Object("cardboard_box", _container_type) + Inside = pred_name_to_pred["Inside"] + return { + GroundAtom(Inside, [blue_block, cardboard_box]), + GroundAtom(Inside, [yellow_cup, cardboard_box]), + GroundAtom(Inside, [toy_plane, cardboard_box]), + } + if goal_description == "put the tennis ball and red ball on the yellow table": + tennis_ball = Object("tennis_ball", _movable_object_type) + red_ball = Object("red_ball", _movable_object_type) + yellow_table = Object("yellow_table", _immovable_object_type) + On = pred_name_to_pred["On"] + Inside = pred_name_to_pred["Inside"] + return { + GroundAtom(On, [tennis_ball, yellow_table]), + GroundAtom(On, [red_ball, yellow_table]), + } + if goal_description == "wipe the wooden table with the sponge": + wooden_table = Object("wooden_table", _immovable_object_type) + sponge = Object("sponge", _movable_object_type) + orange_bucket = Object("orange_bucket", _container_type) + SurfaceWiped = pred_name_to_pred["SurfaceWiped"] + Inside = pred_name_to_pred["Inside"] + return { + GroundAtom(SurfaceWiped, [wooden_table]), + GroundAtom(Inside, [sponge, orange_bucket]), + } if goal_description == "setup sweeping": robot = Object("robot", _robot_type) brush = Object("brush", _movable_object_type) @@ -503,6 +775,42 @@ def _create_goal(self, state: State, GroundAtom(ContainerReadyForSweeping, [bucket, black_table]), GroundAtom(IsSweeper, [brush]) } + if goal_description == "get the cup onto the table!": + robot = Object("robot", _robot_type) + cup = Object("yellow_toy_cup", _movable_object_type) + table = Object("small_cardboard_box_with_black_tape", + _immovable_object_type) + HandEmpty = pred_name_to_pred["HandEmpty"] + VLMOn = pred_name_to_pred["VLMOn"] + goal = { + GroundAtom(HandEmpty, [robot]), + GroundAtom(VLMOn, [cup, table]) + } + return goal + if goal_description == "clean up the table!": + Inside = pred_name_to_pred["VLMIn"] + # TableClean = pred_name_to_pred["TableClean"] + # TableClear = pred_name_to_pred["TableClear"] + TableWiped = pred_name_to_pred["TableWiped"] + # OnFloor = pred_name_to_pred["OnFloor"] + # IsGrumpy = pred_name_to_pred["IsGrumpy"] + # CanBeUsedForErasing = pred_name_to_pred["CanBeUsedForErasing"] + # Holding = pred_name_to_pred["Holding"] + clear_trash_can = Object("clear_plastic_dustbin", _trash_can_type) + cardboard_trash_can = Object("cardboard_box_bin", _trash_can_type) + apple = Object("apple", _movable_object_type) + table = Object("child_play_table", _table_type) + eraser = Object("fluffy_green_toy_eraser", _movable_object_type) + # robot = Object("robot", _robot_type) + goal = { + # GroundAtom(Holding, [robot, eraser]), + GroundAtom(Inside, [eraser, clear_trash_can]), + GroundAtom(Inside, [apple, cardboard_trash_can]), + GroundAtom(TableWiped, [table]), + # GroundAtom(IsGrumpy, [trash_can]), + # GroundAtom(Holding, [robot, apple]), + } + return goal raise NotImplementedError("Unrecognized goal description") def render_mental_images(self, observation: Observation, @@ -590,15 +898,6 @@ class SpotMinimalPerceiver(BasePerceiver): anything about. """ - camera_name_to_annotation = { - 'hand_color_image': "Hand Camera Image", - 'back_fisheye_image': "Back Camera Image", - 'frontleft_fisheye_image': "Front Left Camera Image", - 'frontright_fisheye_image': "Front Right Camera Image", - 'left_fisheye_image': "Left Camera Image", - 'right_fisheye_image': "Right Camera Image" - } - def render_mental_images(self, observation: Observation, env_task: EnvironmentTask) -> Video: raise NotImplementedError() @@ -633,12 +932,15 @@ def _create_goal(self, state: State, # Hopefully one day other cleanups will enable cleaning. assert self._curr_env is not None pred_name_to_pred = {p.name: p for p in self._curr_env.predicates} - Inside = pred_name_to_pred["Inside"] - Holding = pred_name_to_pred["Holding"] - HandEmpty = pred_name_to_pred["HandEmpty"] - VLMOn = pred_name_to_pred["VLMOn"] + try: + Inside = pred_name_to_pred["Inside"] + Holding = pred_name_to_pred["Holding"] + HandEmpty = pred_name_to_pred["HandEmpty"] + except KeyError: + import ipdb; ipdb.set_trace() if goal_description == "get the cup onto the table!": + VLMOn = pred_name_to_pred["VLMOn"] robot = Object("robot", _robot_type) cup = Object("yellow_toy_cup", _movable_object_type) table = Object("cardboard_table", _immovable_object_type) @@ -656,6 +958,23 @@ def _create_goal(self, state: State, GroundAtom(Holding, [robot, dustpan]) } return goal + if goal_description == "clean up the table!": + VLMIn = pred_name_to_pred["VLMIn"] + TableClean = pred_name_to_pred["TableClean"] + TableClear = pred_name_to_pred["TableClear"] + TableWiped = pred_name_to_pred["TableWiped"] + CanBeUsedForErasing = pred_name_to_pred["CanBeUsedForErasing"] + trash_can = Object("clear_plastic_trash_can", + _immovable_object_type) + apple = Object("apple", _movable_object_type) + table = Object("childrens_play_table", _table_type) + eraser = Object("neon_green_fluffy_eraser", _movable_object_type) + robot = Object("robot", _robot_type) + goal = { + GroundAtom(VLMIn, [apple, trash_can]), + GroundAtom(TableWiped, [table]), + } + return goal raise NotImplementedError("Unrecognized goal description") @@ -685,51 +1004,25 @@ def step(self, observation: Observation) -> State: # observation, then update the ordered list of known objects. if self._waiting_for_observation: assert len(self._ordered_objects) == 0 - self._ordered_objects = sorted(observation.all_objects) + # TODO # self._ordered_objects = sorted(observation.all_objects) + self._ordered_objects = [] self._waiting_for_observation = False self._robot = observation.robot - img_objects = observation.rgbd_images # RGBDImage objects - img_names = [v.camera_name for _, v in img_objects.items()] - imgs = [v.rotated_rgb for _, v in img_objects.items()] - pil_imgs = [PIL.Image.fromarray(img) for img in imgs] # type: ignore - # Annotate images with detected objects (names + bounding box) - # and camera name. - object_detections_per_camera = observation.object_detections_per_camera - for i, camera_name in enumerate(img_names): - draw = ImageDraw.Draw(pil_imgs[i]) - # Annotate with camera name. - font = utils.get_scaled_default_font(draw, 4) - _ = utils.add_text_to_draw_img( - draw, (0, 0), self.camera_name_to_annotation[camera_name], - font) - # Annotate with object detections. - detections = object_detections_per_camera[camera_name] - for obj_id, seg_bb in detections: - x0, y0, x1, y1 = seg_bb.bounding_box - x0, x1 = sorted([x0, x1]) - y0, y1 = sorted([y0, y1]) - draw.rectangle([(x0, y0), (x1, y1)], outline='green', width=2) - text = f"{obj_id.language_id}" - font = utils.get_scaled_default_font(draw, 3) - text_mask = font.getmask(text) # type: ignore - text_width, text_height = text_mask.size - text_bbox = [(x0, y0 - 1.5 * text_height), - (x0 + text_width + 1, y0)] - draw.rectangle(text_bbox, fill='green') - draw.text((x0 + 1, y0 - 1.5 * text_height), - text, - fill='white', - font=font) - annotated_imgs = list(pil_imgs) + annotated_imgs = annotate_imgs_with_detections( + observation.rgbd_images, observation.object_detections_per_camera) self._gripper_open_percentage = observation.gripper_open_percentage + # If we're trying to record VLM demos, then save the images. + if len(CFG.spot_vlm_teleop_demo_folderpath) > 0: + save_annotated_imgs_for_vlm_demo( + annotated_imgs, Path(CFG.spot_vlm_teleop_demo_folderpath)) self._curr_state = self._create_state() if observation.executed_skill is not None: if "Pick" in observation.executed_skill.extra_info.action_name: for obj in observation.executed_skill.extra_info.\ operator_objects: - if not obj.is_instance(_robot_type): + if obj.is_instance(_movable_object_type): # Turn the held feature on self._curr_state.set(obj, "held", 1.0) if "Place" in observation.executed_skill.extra_info.action_name: @@ -842,7 +1135,7 @@ def _create_state(self) -> State: "in_view": 0, "is_sweeper": 0, }) - elif obj.type.name == "immovable": + elif obj.type.name in ["immovable", "table"]: state_dict[obj].update({"flat_top_surface": 1}) else: raise ValueError( diff --git a/predicators/planning.py b/predicators/planning.py index c578c1dfd9..b9657a9239 100644 --- a/predicators/planning.py +++ b/predicators/planning.py @@ -322,11 +322,11 @@ def task_plan( in tests/test_planning for usage examples. """ if not goal.issubset(reachable_atoms): - logging.info(f"Detected goal unreachable. Goal: {goal}") - logging.info(f"Initial atoms: {init_atoms}") + logging.info(f"Detected goal unreachable. Goal: {sorted(list(goal))}\n") + logging.info(f"Initial atoms: {sorted(list(init_atoms))}\n") logging.info( - f"Reachable atoms not in init: {reachable_atoms - init_atoms}") - raise PlanningFailure(f"Goal {goal} not dr-reachable") + f"Reachable atoms not in init: {sorted(list(reachable_atoms - init_atoms))}\n") + raise PlanningFailure(f"Goal {sorted(list(goal))} not dr-reachable\n") dummy_task = Task(DefaultState, goal) metrics: Metrics = defaultdict(float) generator = _skeleton_generator( @@ -1197,7 +1197,6 @@ def run_task_plan_once( init_atoms = utils.abstract(task.init, preds) goal = task.goal objects = set(task.init) - start_time = time.perf_counter() if CFG.sesame_task_planner == "astar": diff --git a/predicators/pretrained_model_interface.py b/predicators/pretrained_model_interface.py index 2a94e930a7..d25da05e18 100644 --- a/predicators/pretrained_model_interface.py +++ b/predicators/pretrained_model_interface.py @@ -117,12 +117,16 @@ def sample_completions(self, os.makedirs(imgs_folderpath, exist_ok=True) for i, img in enumerate(imgs): filename_suffix = str(i) + ".jpg" + # Convert RGBA to RGB since JPEG doesn't support alpha + if img.mode == 'RGBA': + img = img.convert('RGB') img.save(os.path.join(imgs_folderpath, filename_suffix)) logging.debug(f"Saved model response to {cache_filepath}.") # Load the saved completion. with open(cache_filepath, 'r', encoding='utf-8') as f: cache_str = f.read() - logging.debug(f"Loaded model response from {cache_filepath}.") + # logging.debug(f"Loaded model response from {cache_filepath}.") + print(f"Loaded model response from {cache_filepath}.") assert cache_str.count(_CACHE_SEP) == num_completions cached_prompt, completion_strs = cache_str.split(_CACHE_SEP, 1) assert cached_prompt == prompt diff --git a/predicators/settings.py b/predicators/settings.py index 68fe316ac0..4310b6c28c 100644 --- a/predicators/settings.py +++ b/predicators/settings.py @@ -186,6 +186,7 @@ class GlobalSettings: spot_run_dry = False spot_use_perfect_samplers = False # for debugging spot_sweep_env_goal_description = "get the objects into the bucket" + spot_vlm_teleop_demo_folderpath = "" # pddl blocks env parameters pddl_blocks_procedural_train_min_num_blocks = 3 @@ -435,7 +436,7 @@ class GlobalSettings: override_json_with_input = False # Only works with SpotEnv for now # parameters for vision language models - # gemini-1.5-pro-latest, gemini-1.5-pro-flash gpt-4-turbo, gpt-4o + # gemini-1.5-pro, gemini-1.5-flash, gpt-4-turbo, gpt-4o # NOTE: we need to create a dummy vlm so that tests on CI pass. vlm_model_name = "dummy" vlm_temperature = 0.0 diff --git a/predicators/spot_utils/README.md b/predicators/spot_utils/README.md index 45e743651e..6524673f7a 100644 --- a/predicators/spot_utils/README.md +++ b/predicators/spot_utils/README.md @@ -25,7 +25,7 @@ python predicators/main.py --env --approach "spot_wrapper[oracle]" -- python predicators/main.py --spot_robot_ip 192.168.80.3 --spot_graph_nav_map b45-621 --env lis_spot_block_floor_env --approach spot_wrapper[oracle] --bilevel_plan_without_sim True --seed 0 # an example to run LIS spot without a map -python predicators/main.py --env spot_vlm_cup_table_env --approach "spot_wrapper[oracle]" --seed 0 --num_train_tasks 0 --num_test_tasks 1 --spot_robot_ip 192.168.80.3 --perceiver spot_minimal_perceiver --bilevel_plan_without_sim True --vlm_test_time_atom_label_prompt_type img_option_diffs_label_history --vlm_model_name gpt-4o --execution_monitor expected_atoms +python predicators/main.py --env spot_vlm_simple_cup_table_env --approach "spot_wrapper[oracle]" --seed 0 --num_train_tasks 0 --num_test_tasks 1 --spot_robot_ip 192.168.80.3 --perceiver spot_minimal_perceiver --bilevel_plan_without_sim True --vlm_test_time_atom_label_prompt_type img_option_diffs_label_history --vlm_model_name gpt-4o --execution_monitor expected_atoms ``` ### Implement Your Task diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_afoul-eel-VMlsWuUtsMDVqYYD2cUO2A== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_afoul-eel-VMlsWuUtsMDVqYYD2cUO2A== new file mode 100644 index 0000000000..3144a6a0c3 --- /dev/null +++ b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_afoul-eel-VMlsWuUtsMDVqYYD2cUO2A== @@ -0,0 +1,2 @@ + +3edge_snapshot_id_afoul-eel-VMlsWuUtsMDVqYYD2cUO2A== \ No newline at end of file diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_amused-merino-kuKz662SjKLa20fo7shs4w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_amused-merino-kuKz662SjKLa20fo7shs4w== new file mode 100644 index 0000000000..4e9389167d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_amused-merino-kuKz662SjKLa20fo7shs4w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_amused-toucan-7L6UAw1SwB3law0WHA5IPQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_amused-toucan-7L6UAw1SwB3law0WHA5IPQ== new file mode 100644 index 0000000000..6cd1a1af09 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_amused-toucan-7L6UAw1SwB3law0WHA5IPQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_antsy-salmon-IkecDl7SLFW52BsmxxpwBA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_antsy-salmon-IkecDl7SLFW52BsmxxpwBA== new file mode 100644 index 0000000000..9fe9e8b118 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_antsy-salmon-IkecDl7SLFW52BsmxxpwBA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_apish-rhesus-TxGxqAlAT9opfeXJ2hTlWw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_apish-rhesus-TxGxqAlAT9opfeXJ2hTlWw== new file mode 100644 index 0000000000..715dda21de Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_apish-rhesus-TxGxqAlAT9opfeXJ2hTlWw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_arrant-lizard-KDdB1+i8FstwS9GsMYjMFA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_arrant-lizard-KDdB1+i8FstwS9GsMYjMFA== new file mode 100644 index 0000000000..5a3caa8e14 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_arrant-lizard-KDdB1+i8FstwS9GsMYjMFA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_atonal-carp-xP6gFxDfmUsZ6wN.nBFM5g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_atonal-carp-xP6gFxDfmUsZ6wN.nBFM5g== new file mode 100644 index 0000000000..14c8c9daf7 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_atonal-carp-xP6gFxDfmUsZ6wN.nBFM5g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_azure-vole-CxeuwaiJFb.XT6ZGu0nPDA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_azure-vole-CxeuwaiJFb.XT6ZGu0nPDA== new file mode 100644 index 0000000000..e6352c8eba Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_azure-vole-CxeuwaiJFb.XT6ZGu0nPDA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bandy-mudcat-swooT02aWnz.OeBc+vKg9Q== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bandy-mudcat-swooT02aWnz.OeBc+vKg9Q== new file mode 100644 index 0000000000..2595304250 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bandy-mudcat-swooT02aWnz.OeBc+vKg9Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bass-jaguar-0JtCXSxiN994YvwCBd+2GQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bass-jaguar-0JtCXSxiN994YvwCBd+2GQ== new file mode 100644 index 0000000000..e30f225067 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bass-jaguar-0JtCXSxiN994YvwCBd+2GQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bay-filly-98yykbCaFnmTI+iPjxuN5Q== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bay-filly-98yykbCaFnmTI+iPjxuN5Q== new file mode 100644 index 0000000000..93c41c5bcb Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_bay-filly-98yykbCaFnmTI+iPjxuN5Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_beige-ant-YAbOX6Bw4U1aOW4.yd+X1g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_beige-ant-YAbOX6Bw4U1aOW4.yd+X1g== new file mode 100644 index 0000000000..86d75be77e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_beige-ant-YAbOX6Bw4U1aOW4.yd+X1g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_biface-setter-XZzzsuYbahSFfxnpapGaag== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_biface-setter-XZzzsuYbahSFfxnpapGaag== new file mode 100644 index 0000000000..96cad99086 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_biface-setter-XZzzsuYbahSFfxnpapGaag== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_boring-leech-MbH1Bjs0sZd5TCQiVVqbnw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_boring-leech-MbH1Bjs0sZd5TCQiVVqbnw== new file mode 100644 index 0000000000..bf740156a0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_boring-leech-MbH1Bjs0sZd5TCQiVVqbnw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_boss-eland-TnPdLcXvNxNPX+EYJ.g2ZA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_boss-eland-TnPdLcXvNxNPX+EYJ.g2ZA== new file mode 100644 index 0000000000..e9ae56a35c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_boss-eland-TnPdLcXvNxNPX+EYJ.g2ZA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_classy-lamb-MHrHEsdZWaBioIKQsJxXPQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_classy-lamb-MHrHEsdZWaBioIKQsJxXPQ== new file mode 100644 index 0000000000..8feba26281 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_classy-lamb-MHrHEsdZWaBioIKQsJxXPQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_cruddy-jaguar-nEIMrD09EPxzjC4nw6mjLA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_cruddy-jaguar-nEIMrD09EPxzjC4nw6mjLA== new file mode 100644 index 0000000000..a4625c1475 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_cruddy-jaguar-nEIMrD09EPxzjC4nw6mjLA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dapper-racoon-iy2MGSfHpFzYvMjTNLon8g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dapper-racoon-iy2MGSfHpFzYvMjTNLon8g== new file mode 100644 index 0000000000..5e93f287a4 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dapper-racoon-iy2MGSfHpFzYvMjTNLon8g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dexter-pug-RRwiaZutHFJYt12pl9KWSA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dexter-pug-RRwiaZutHFJYt12pl9KWSA== new file mode 100644 index 0000000000..d22dca7eec Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dexter-pug-RRwiaZutHFJYt12pl9KWSA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dipped-camel-zU6iINm9Tc3zT2Ya9QFKVw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dipped-camel-zU6iINm9Tc3zT2Ya9QFKVw== new file mode 100644 index 0000000000..3db3588a64 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_dipped-camel-zU6iINm9Tc3zT2Ya9QFKVw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_domed-badger-d8QsWecQE9XKklk1IcyX1g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_domed-badger-d8QsWecQE9XKklk1IcyX1g== new file mode 100644 index 0000000000..095f0b84f5 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_domed-badger-d8QsWecQE9XKklk1IcyX1g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_ended-aphid-PR1NL1ED3GmtbQ0OxFVoTg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_ended-aphid-PR1NL1ED3GmtbQ0OxFVoTg== new file mode 100644 index 0000000000..4db132738a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_ended-aphid-PR1NL1ED3GmtbQ0OxFVoTg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_erring-ibis-JEse8nN+TZLKmvNbfwyhOg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_erring-ibis-JEse8nN+TZLKmvNbfwyhOg== new file mode 100644 index 0000000000..1dbb6cd107 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_erring-ibis-JEse8nN+TZLKmvNbfwyhOg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_feral-carp-JyYBbhoxdSI2KIqKDuEjKg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_feral-carp-JyYBbhoxdSI2KIqKDuEjKg== new file mode 100644 index 0000000000..3ee23fd71a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_feral-carp-JyYBbhoxdSI2KIqKDuEjKg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_festal-deer-g+8iP0Qdo5yYL7kkG9X+sg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_festal-deer-g+8iP0Qdo5yYL7kkG9X+sg== new file mode 100644 index 0000000000..5b9a1af4fc Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_festal-deer-g+8iP0Qdo5yYL7kkG9X+sg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_filial-maggot-UbMRGemiGlgYS71ibGSZrA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_filial-maggot-UbMRGemiGlgYS71ibGSZrA== new file mode 100644 index 0000000000..0a251e60c9 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_filial-maggot-UbMRGemiGlgYS71ibGSZrA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_fogged-bison-14RMkfTKEqsnhLPuvRY0bA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_fogged-bison-14RMkfTKEqsnhLPuvRY0bA== new file mode 100644 index 0000000000..fc3eda058f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_fogged-bison-14RMkfTKEqsnhLPuvRY0bA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_frizzy-skate-47zX0pJO9PSnLeJlRVD.Xw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_frizzy-skate-47zX0pJO9PSnLeJlRVD.Xw== new file mode 100644 index 0000000000..9965c904a8 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_frizzy-skate-47zX0pJO9PSnLeJlRVD.Xw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_frugal-puffin-uke3NpPQBux6Ss.qLpRIJQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_frugal-puffin-uke3NpPQBux6Ss.qLpRIJQ== new file mode 100644 index 0000000000..b179ccc21e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_frugal-puffin-uke3NpPQBux6Ss.qLpRIJQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_gelid-rat-d1aRhB1u75kPulmRu.zZpw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_gelid-rat-d1aRhB1u75kPulmRu.zZpw== new file mode 100644 index 0000000000..37e85c455f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_gelid-rat-d1aRhB1u75kPulmRu.zZpw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_glace-raven-o2s5tCVHBibZaIlQTfNgnA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_glace-raven-o2s5tCVHBibZaIlQTfNgnA== new file mode 100644 index 0000000000..7622335e45 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_glace-raven-o2s5tCVHBibZaIlQTfNgnA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_hectic-mantis-+0o35jWqkaWZl7sKPEINcw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_hectic-mantis-+0o35jWqkaWZl7sKPEINcw== new file mode 100644 index 0000000000..7338d880d1 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_hectic-mantis-+0o35jWqkaWZl7sKPEINcw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_holy-sloth-4eAZXiTpx7wZzhPn7yy0AA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_holy-sloth-4eAZXiTpx7wZzhPn7yy0AA== new file mode 100644 index 0000000000..41193cc82c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_holy-sloth-4eAZXiTpx7wZzhPn7yy0AA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_homy-puffer-Lv0qS5QM.1PEketJERu0fg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_homy-puffer-Lv0qS5QM.1PEketJERu0fg== new file mode 100644 index 0000000000..eecf244662 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_homy-puffer-Lv0qS5QM.1PEketJERu0fg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_indie-crake-ylVkPcD01sDeASbMSsYyDQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_indie-crake-ylVkPcD01sDeASbMSsYyDQ== new file mode 100644 index 0000000000..f43aefded1 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_indie-crake-ylVkPcD01sDeASbMSsYyDQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_jet-borzoi-FWtS46dtQnUjfdiiFBl5dA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_jet-borzoi-FWtS46dtQnUjfdiiFBl5dA== new file mode 100644 index 0000000000..d0ad1b8c2c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_jet-borzoi-FWtS46dtQnUjfdiiFBl5dA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_knobby-gerbil-.ZoumBvX1lHIoy2p.DqRyw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_knobby-gerbil-.ZoumBvX1lHIoy2p.DqRyw== new file mode 100644 index 0000000000..7c92662e35 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_knobby-gerbil-.ZoumBvX1lHIoy2p.DqRyw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_laic-shrimp-ExFdCjo2Tmvp23doqMIQyA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_laic-shrimp-ExFdCjo2Tmvp23doqMIQyA== new file mode 100644 index 0000000000..95bca91c0d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_laic-shrimp-ExFdCjo2Tmvp23doqMIQyA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_legato-cowrie-C6lrQ3tIRuw0uJhb0wyttg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_legato-cowrie-C6lrQ3tIRuw0uJhb0wyttg== new file mode 100644 index 0000000000..9fb6529854 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_legato-cowrie-C6lrQ3tIRuw0uJhb0wyttg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_lethal-goose-ILvhjapOR+YN4mJyG98fSQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_lethal-goose-ILvhjapOR+YN4mJyG98fSQ== new file mode 100644 index 0000000000..2988d6f7c4 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_lethal-goose-ILvhjapOR+YN4mJyG98fSQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_liked-hyla-UnBfVl27J59dEhLbdi8spQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_liked-hyla-UnBfVl27J59dEhLbdi8spQ== new file mode 100644 index 0000000000..b0fd0eb14c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_liked-hyla-UnBfVl27J59dEhLbdi8spQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_listed-gnu-F+dR9BTwuHYS9P+ksd6Urg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_listed-gnu-F+dR9BTwuHYS9P+ksd6Urg== new file mode 100644 index 0000000000..44284c433f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_listed-gnu-F+dR9BTwuHYS9P+ksd6Urg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_loco-mule-2smJTL8ESSnhY6Z3bgiI0g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_loco-mule-2smJTL8ESSnhY6Z3bgiI0g== new file mode 100644 index 0000000000..b06260892d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_loco-mule-2smJTL8ESSnhY6Z3bgiI0g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_marred-dove-PucYsyjuck2TPRHBo.YCBg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_marred-dove-PucYsyjuck2TPRHBo.YCBg== new file mode 100644 index 0000000000..7b6e6ec8b3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_marred-dove-PucYsyjuck2TPRHBo.YCBg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mat-thrush-.BkDOYfG5R3gH9.lB+X3+g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mat-thrush-.BkDOYfG5R3gH9.lB+X3+g== new file mode 100644 index 0000000000..61ca4c8d39 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mat-thrush-.BkDOYfG5R3gH9.lB+X3+g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_matt-seal-2YP.uU1Cpp.5qvHaGEJheQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_matt-seal-2YP.uU1Cpp.5qvHaGEJheQ== new file mode 100644 index 0000000000..0e372e8e1b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_matt-seal-2YP.uU1Cpp.5qvHaGEJheQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_meet-fawn-n7R4Hv36M59.by77Yd+2pQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_meet-fawn-n7R4Hv36M59.by77Yd+2pQ== new file mode 100644 index 0000000000..280a04e7fe Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_meet-fawn-n7R4Hv36M59.by77Yd+2pQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_menial-mink-x7bJVotQOUUfFQi+V1XNWA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_menial-mink-x7bJVotQOUUfFQi+V1XNWA== new file mode 100644 index 0000000000..ed61557fb1 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_menial-mink-x7bJVotQOUUfFQi+V1XNWA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_messy-dugong-YSr2E.mEjYBFSRqkr3vxbg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_messy-dugong-YSr2E.mEjYBFSRqkr3vxbg== new file mode 100644 index 0000000000..67eb80934a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_messy-dugong-YSr2E.mEjYBFSRqkr3vxbg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mined-ewe-kOc6t5O+ZoflY7mvqlxlUg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mined-ewe-kOc6t5O+ZoflY7mvqlxlUg== new file mode 100644 index 0000000000..bdcd66f483 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mined-ewe-kOc6t5O+ZoflY7mvqlxlUg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_miry-weevil-Y2JEOzjcCPjKHtwlVOk4nw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_miry-weevil-Y2JEOzjcCPjKHtwlVOk4nw== new file mode 100644 index 0000000000..c3e450c2a3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_miry-weevil-Y2JEOzjcCPjKHtwlVOk4nw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_molar-fly-HtkxrGlYKQxmU0CprZ9pkw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_molar-fly-HtkxrGlYKQxmU0CprZ9pkw== new file mode 100644 index 0000000000..3eab673535 --- /dev/null +++ b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_molar-fly-HtkxrGlYKQxmU0CprZ9pkw== @@ -0,0 +1,2 @@ + +3edge_snapshot_id_molar-fly-HtkxrGlYKQxmU0CprZ9pkw== \ No newline at end of file diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_monied-barker-7uQ+bpLT2ex.M8xDy7ltUw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_monied-barker-7uQ+bpLT2ex.M8xDy7ltUw== new file mode 100644 index 0000000000..56e4777570 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_monied-barker-7uQ+bpLT2ex.M8xDy7ltUw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_musing-cuscus-5tSNTcrFowjlR1wCeg7rXw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_musing-cuscus-5tSNTcrFowjlR1wCeg7rXw== new file mode 100644 index 0000000000..7aea61214b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_musing-cuscus-5tSNTcrFowjlR1wCeg7rXw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_musky-lion-EK+F4vEUuvJGpw6ZVW0mCg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_musky-lion-EK+F4vEUuvJGpw6ZVW0mCg== new file mode 100644 index 0000000000..3b071773a3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_musky-lion-EK+F4vEUuvJGpw6ZVW0mCg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mythic-sawfly-A2krt0kIZNFrpI0YQU5Rdw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mythic-sawfly-A2krt0kIZNFrpI0YQU5Rdw== new file mode 100644 index 0000000000..262be373d9 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_mythic-sawfly-A2krt0kIZNFrpI0YQU5Rdw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_orange-koodoo-zMAEsr2.y1Kpc5mrNlL+8w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_orange-koodoo-zMAEsr2.y1Kpc5mrNlL+8w== new file mode 100644 index 0000000000..2e2a8f83f4 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_orange-koodoo-zMAEsr2.y1Kpc5mrNlL+8w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_palmy-giant-ucmEzjIwQbCiH8bs.aElEA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_palmy-giant-ucmEzjIwQbCiH8bs.aElEA== new file mode 100644 index 0000000000..29564660b3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_palmy-giant-ucmEzjIwQbCiH8bs.aElEA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pasted-equid-2rLVH+n+nLYWYh14rTScVg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pasted-equid-2rLVH+n+nLYWYh14rTScVg== new file mode 100644 index 0000000000..26f38fb4b5 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pasted-equid-2rLVH+n+nLYWYh14rTScVg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_paved-egg-5YMnab.roB3h1tTlnApw2A== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_paved-egg-5YMnab.roB3h1tTlnApw2A== new file mode 100644 index 0000000000..02b196e1da Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_paved-egg-5YMnab.roB3h1tTlnApw2A== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pebbly-baboon-ARvbrQpvZB6gV8wjo0rtxQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pebbly-baboon-ARvbrQpvZB6gV8wjo0rtxQ== new file mode 100644 index 0000000000..92b7425acf Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pebbly-baboon-ARvbrQpvZB6gV8wjo0rtxQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pimply-burro-a+84Se.4+yjYQYCNuQ1fGw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pimply-burro-a+84Se.4+yjYQYCNuQ1fGw== new file mode 100644 index 0000000000..976bad0433 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pimply-burro-a+84Se.4+yjYQYCNuQ1fGw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_porose-fish-dmIJ2UzuSMWFxGhDeRBF.A== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_porose-fish-dmIJ2UzuSMWFxGhDeRBF.A== new file mode 100644 index 0000000000..bfa1faf632 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_porose-fish-dmIJ2UzuSMWFxGhDeRBF.A== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_posh-pike-R3QBi9scZbjiZUBOpJI0.A== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_posh-pike-R3QBi9scZbjiZUBOpJI0.A== new file mode 100644 index 0000000000..9f15a948dc --- /dev/null +++ b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_posh-pike-R3QBi9scZbjiZUBOpJI0.A== @@ -0,0 +1,2 @@ + +3edge_snapshot_id_posh-pike-R3QBi9scZbjiZUBOpJI0.A== \ No newline at end of file diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pricy-worm-z.uHD9x3DurOWB3qzlbcEQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pricy-worm-z.uHD9x3DurOWB3qzlbcEQ== new file mode 100644 index 0000000000..40dc47eb50 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_pricy-worm-z.uHD9x3DurOWB3qzlbcEQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_proto-hare-1gR8qRwQP7Xaq4WKjmJ5AQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_proto-hare-1gR8qRwQP7Xaq4WKjmJ5AQ== new file mode 100644 index 0000000000..b038a179a4 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_proto-hare-1gR8qRwQP7Xaq4WKjmJ5AQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_ruby-setter-wXVvrfpD1Rvx7XRUpv1DXw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_ruby-setter-wXVvrfpD1Rvx7XRUpv1DXw== new file mode 100644 index 0000000000..8b3431f3bd Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_ruby-setter-wXVvrfpD1Rvx7XRUpv1DXw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_rueful-fleece-qSZ.48qgB7kTB9pq0p5Klw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_rueful-fleece-qSZ.48qgB7kTB9pq0p5Klw== new file mode 100644 index 0000000000..a17e31a375 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_rueful-fleece-qSZ.48qgB7kTB9pq0p5Klw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_senile-drone-z.FXOe76l1TxPQKUjitE4w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_senile-drone-z.FXOe76l1TxPQKUjitE4w== new file mode 100644 index 0000000000..794d9cfd7a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_senile-drone-z.FXOe76l1TxPQKUjitE4w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_skewed-jay-6KTZoqJA+PUdu.l28Q4u.A== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_skewed-jay-6KTZoqJA+PUdu.l28Q4u.A== new file mode 100644 index 0000000000..d9f88d1aad Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_skewed-jay-6KTZoqJA+PUdu.l28Q4u.A== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_smarmy-hogg-clZhtZQpDoUVZfVy8+4kbQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_smarmy-hogg-clZhtZQpDoUVZfVy8+4kbQ== new file mode 100644 index 0000000000..3133e2364f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_smarmy-hogg-clZhtZQpDoUVZfVy8+4kbQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snaky-bison-hJ37wNAAe54MamYmmKR5ug== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snaky-bison-hJ37wNAAe54MamYmmKR5ug== new file mode 100644 index 0000000000..e9cd3bfed8 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snaky-bison-hJ37wNAAe54MamYmmKR5ug== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snaky-puffin-m5S2UjcQneeuR92XxRBNVQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snaky-puffin-m5S2UjcQneeuR92XxRBNVQ== new file mode 100644 index 0000000000..8271643005 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snaky-puffin-m5S2UjcQneeuR92XxRBNVQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snuffed-lizard-BT3olZzMzDsarNSH7AI1vA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snuffed-lizard-BT3olZzMzDsarNSH7AI1vA== new file mode 100644 index 0000000000..4bb707934a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_snuffed-lizard-BT3olZzMzDsarNSH7AI1vA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_soaked-hyena-DzytZLdZ8SOwB47U2EONHQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_soaked-hyena-DzytZLdZ8SOwB47U2EONHQ== new file mode 100644 index 0000000000..6522f17392 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_soaked-hyena-DzytZLdZ8SOwB47U2EONHQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_soured-ewe-nJAfVzhlPyLHu9NuHMpeNQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_soured-ewe-nJAfVzhlPyLHu9NuHMpeNQ== new file mode 100644 index 0000000000..6a05ee0109 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_soured-ewe-nJAfVzhlPyLHu9NuHMpeNQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_surly-hound-3hMDZ0RxvgYj.792yKJ6FQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_surly-hound-3hMDZ0RxvgYj.792yKJ6FQ== new file mode 100644 index 0000000000..e24de5ca5e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_surly-hound-3hMDZ0RxvgYj.792yKJ6FQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tailed-minnow-dTwpoe5qNQZqsYygiTGqPA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tailed-minnow-dTwpoe5qNQZqsYygiTGqPA== new file mode 100644 index 0000000000..13675bd205 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tailed-minnow-dTwpoe5qNQZqsYygiTGqPA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_titled-mamba-c1s+cZnFt2U5Uw0ThRvrgA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_titled-mamba-c1s+cZnFt2U5Uw0ThRvrgA== new file mode 100644 index 0000000000..77b21903fc Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_titled-mamba-c1s+cZnFt2U5Uw0ThRvrgA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_togged-weevil-YGm+5jdwuEjq5fcS6a26zw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_togged-weevil-YGm+5jdwuEjq5fcS6a26zw== new file mode 100644 index 0000000000..8e72559345 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_togged-weevil-YGm+5jdwuEjq5fcS6a26zw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tonal-squid-5dGMfHfz+AhkxMO3VpcqsA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tonal-squid-5dGMfHfz+AhkxMO3VpcqsA== new file mode 100644 index 0000000000..e7151c516a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tonal-squid-5dGMfHfz+AhkxMO3VpcqsA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tusked-goose-mwKHbbx5b43GBZSVbMNpPA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tusked-goose-mwKHbbx5b43GBZSVbMNpPA== new file mode 100644 index 0000000000..ef09b1c1ac Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_tusked-goose-mwKHbbx5b43GBZSVbMNpPA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_twisty-pincer-vmafDzPMNwX7BuPH+K3xDw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_twisty-pincer-vmafDzPMNwX7BuPH+K3xDw== new file mode 100644 index 0000000000..a6ea63c0ea Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_twisty-pincer-vmafDzPMNwX7BuPH+K3xDw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unaged-bird-b0080zOnOY1MITWElGyadQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unaged-bird-b0080zOnOY1MITWElGyadQ== new file mode 100644 index 0000000000..882509c704 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unaged-bird-b0080zOnOY1MITWElGyadQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_undyed-gnu-PlaHoDy9DJwuMGO4ibz4hA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_undyed-gnu-PlaHoDy9DJwuMGO4ibz4hA== new file mode 100644 index 0000000000..b65922f5a0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_undyed-gnu-PlaHoDy9DJwuMGO4ibz4hA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unhewn-snail-P+CQ1KdKJ0vNa9G9woDnmg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unhewn-snail-P+CQ1KdKJ0vNa9G9woDnmg== new file mode 100644 index 0000000000..ed9fb317fc Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unhewn-snail-P+CQ1KdKJ0vNa9G9woDnmg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unsaid-boxer-i6JtFsp.ChLz57ut2KMlsA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unsaid-boxer-i6JtFsp.ChLz57ut2KMlsA== new file mode 100644 index 0000000000..2e79a29b5b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unsaid-boxer-i6JtFsp.ChLz57ut2KMlsA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unsaid-kiwi-VsgjZDEH2MmjrQfkBeRvLQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unsaid-kiwi-VsgjZDEH2MmjrQfkBeRvLQ== new file mode 100644 index 0000000000..1193527ca6 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_unsaid-kiwi-VsgjZDEH2MmjrQfkBeRvLQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_upmost-conger-hX28x3JhYKm8Tzb8DhHfLw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_upmost-conger-hX28x3JhYKm8Tzb8DhHfLw== new file mode 100644 index 0000000000..b65f36092d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_upmost-conger-hX28x3JhYKm8Tzb8DhHfLw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_upmost-narwal-Pa0XKHZAB5goXjByazeNhw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_upmost-narwal-Pa0XKHZAB5goXjByazeNhw== new file mode 100644 index 0000000000..bb9bfc51c1 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_upmost-narwal-Pa0XKHZAB5goXjByazeNhw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_uppish-biped-CTvotE5zum9RIZ9XkFXZ3Q== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_uppish-biped-CTvotE5zum9RIZ9XkFXZ3Q== new file mode 100644 index 0000000000..bdf7a25f95 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_uppish-biped-CTvotE5zum9RIZ9XkFXZ3Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_vented-oxtail-IWlnCGcg58ABIOe9xTKpzA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_vented-oxtail-IWlnCGcg58ABIOe9xTKpzA== new file mode 100644 index 0000000000..7c5898a589 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_vented-oxtail-IWlnCGcg58ABIOe9xTKpzA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_voiced-skunk-NgBVcqr1YuUiJWyYrOMXvA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_voiced-skunk-NgBVcqr1YuUiJWyYrOMXvA== new file mode 100644 index 0000000000..6ac19998b2 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_voiced-skunk-NgBVcqr1YuUiJWyYrOMXvA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_warty-tick-Z4Zeu5BpbL0WygUORbt5UA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_warty-tick-Z4Zeu5BpbL0WygUORbt5UA== new file mode 100644 index 0000000000..a9b936a3f0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_warty-tick-Z4Zeu5BpbL0WygUORbt5UA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_wedged-nasua-P2w11lhDcWB+kVQAneJ2NA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_wedged-nasua-P2w11lhDcWB+kVQAneJ2NA== new file mode 100644 index 0000000000..193cc60c91 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_wedged-nasua-P2w11lhDcWB+kVQAneJ2NA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_won-scarab-RJmSdKNwGx3oen.1BgQrNw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_won-scarab-RJmSdKNwGx3oen.1BgQrNw== new file mode 100644 index 0000000000..5f2607e543 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_won-scarab-RJmSdKNwGx3oen.1BgQrNw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_wordy-foal-otQTYuYxuaRaPmNAY.xFVg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_wordy-foal-otQTYuYxuaRaPmNAY.xFVg== new file mode 100644 index 0000000000..6f47c351b9 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_wordy-foal-otQTYuYxuaRaPmNAY.xFVg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_woven-horse-qpJ63fZ+6eMVZLXIizEBng== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_woven-horse-qpJ63fZ+6eMVZLXIizEBng== new file mode 100644 index 0000000000..ad5727faf8 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/edge_snapshots/edge_snapshot_id_woven-horse-qpJ63fZ+6eMVZLXIizEBng== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/graph b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/graph new file mode 100644 index 0000000000..d85f59c083 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/graph differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/metadata.yaml b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/metadata.yaml new file mode 100644 index 0000000000..b452cba04d --- /dev/null +++ b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/metadata.yaml @@ -0,0 +1,147 @@ +# Additional info associated with the map. +--- +# NOTE: close to the center of the room +#spot-home-pose: +# x: 2.85 +# y: 0.0 +# angle: -1.45769 +# NOTE: a place closer to the corner that the cable can reach - easier for debug +spot-home-pose: + x: 3.5 + y: 0.45 + angle: 0. +april-tag-offsets: [] + +# Allowed regions. Each region is defined by a set of points +# that form the boundary of the region. We will check +# whether a pose is within the region by checking whether the +# robot will be within the convex hull of these boundary +# points. +allowed-regions: + spot-room: + - [0.25, 1.9] + - [1.2, -2.5] + - [5.7, -1.5] + - [5.0, 2.7] + +# Known immovable objects. Assuming default rotations. +# TODO note: later we can add static objects like short_round_coffee_tables, chairs, etc. here +known-immovable-objects: + floor: + x: 1.4 + y: 0.5 + z: -0.5 + # short_round_coffee_table: + # x: 1.65 + # y: -0.24 + # z: 0.14 + child_play_table: + x: 1.65 + y: -0.24 + z: 0.14 + +# Static object features, including the shapes and sizes of known objects. +static-object-features: + floor: + shape: 1 + height: 0.0001 + length: 10000000 # effectively infinite + width: 10000000 + flat_top_surface: 1 + red_block: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + yellow_toy_cup: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + small_cardboard_box_with_black_tape: + shape: 1 # cuboid + height: 0.55 + length: 0.28 + width: 0.6 + flat_top_surface: 1 + apple: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + blue_block: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + blue_coffee_cup: + shape: 1 + height: 0.2 + length: 0.2 + width: 0.2 + placeable: 1 + is_sweeper: 0 + clear_plastic_dustbin: + shape: 2 + height: 0.1 + length: 0.2 + width: 0.2 + placeable: 1 + is_sweeper: 0 + flat_top_surface: 0 + cardboard_box_bin: + shape: 2 + height: 0.1 + length: 0.2 + width: 0.2 + placeable: 1 + is_sweeper: 0 + flat_top_surface: 0 + fluffy_green_toy_eraser: + shape: 1 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + short_round_coffee_table: + shape: 1 + height: 0.1 + length: 0.79 + width: 0.62 + flat_top_surface: 1 + child_play_table: + shape: 1 + height: 0.1 + length: 0.79 + width: 0.62 + flat_top_surface: 1 + +# Helpful for static objects that are up against a wall, for example. +approach_angle_bounds: + # only approach the apple from in front of it! + apple: [-0.1, 0.1] + # only approach the coffee cup from in front of it! + blue_coffee_cup: [-0.1, 0.1] + # also only approach the short_round_coffee_table from in front of it! + short_round_coffee_table: [-0.1, 0.1] + # only approach the child_play_table from in front of it! + child_play_table: [-0.1, 0.1] + # approach trash can from the side + clear_plastic_dustbin: [1.47, 1.67] + cardboard_box_bin: [1.47, 1.67] + # approach the duster from the other side. + fluffy_green_toy_eraser: [-0.1, 0.1] #[-1.67, -1.47] + +# Wiping skill params. +wipe_location: + short_round_coffee_table: [2.48, 0.205, -2.96856] + child_play_table: [2.48, 0.205, -2.96856] \ No newline at end of file diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_achy-bear-doFNB2AA9gP289SzH9fAWw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_achy-bear-doFNB2AA9gP289SzH9fAWw== new file mode 100644 index 0000000000..935f150e5a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_achy-bear-doFNB2AA9gP289SzH9fAWw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_adagio-merlin-6XsQ7HmxYXX.SGpN0k3ASw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_adagio-merlin-6XsQ7HmxYXX.SGpN0k3ASw== new file mode 100644 index 0000000000..d5e2736891 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_adagio-merlin-6XsQ7HmxYXX.SGpN0k3ASw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_adept-badger-Xr4KX3+RUaGpNdgdogVyKg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_adept-badger-Xr4KX3+RUaGpNdgdogVyKg== new file mode 100644 index 0000000000..90d8ec4b12 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_adept-badger-Xr4KX3+RUaGpNdgdogVyKg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_aglow-drake-iZEbqCuJYJdi4zZFklJWgA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_aglow-drake-iZEbqCuJYJdi4zZFklJWgA== new file mode 100644 index 0000000000..0a4d59973c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_aglow-drake-iZEbqCuJYJdi4zZFklJWgA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_akimbo-conger-JPfLF918YLYhDHsrmRhWWg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_akimbo-conger-JPfLF918YLYhDHsrmRhWWg== new file mode 100644 index 0000000000..9b747c5732 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_akimbo-conger-JPfLF918YLYhDHsrmRhWWg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_amber-corgi-fJoRq2jb9Nxx0NqaPJ2S0Q== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_amber-corgi-fJoRq2jb9Nxx0NqaPJ2S0Q== new file mode 100644 index 0000000000..860188a89a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_amber-corgi-fJoRq2jb9Nxx0NqaPJ2S0Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_apodal-avocet-WCpCU5udqwiJcKKqE7oSKA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_apodal-avocet-WCpCU5udqwiJcKKqE7oSKA== new file mode 100644 index 0000000000..591f80006d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_apodal-avocet-WCpCU5udqwiJcKKqE7oSKA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_arcane-antler-4JZgMUXrLAT+AvbeM078uQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_arcane-antler-4JZgMUXrLAT+AvbeM078uQ== new file mode 100644 index 0000000000..ef59d210c2 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_arcane-antler-4JZgMUXrLAT+AvbeM078uQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_arty-redbug-i3NWfzT9DM99YzmJCYTtGQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_arty-redbug-i3NWfzT9DM99YzmJCYTtGQ== new file mode 100644 index 0000000000..5b691b080f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_arty-redbug-i3NWfzT9DM99YzmJCYTtGQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bandy-dugong-jjCZXqMoYHugeXC0QiW1mw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bandy-dugong-jjCZXqMoYHugeXC0QiW1mw== new file mode 100644 index 0000000000..155a4c3493 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bandy-dugong-jjCZXqMoYHugeXC0QiW1mw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bibbed-guinea-nRbpuKhwfQpLdUktQC3CHA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bibbed-guinea-nRbpuKhwfQpLdUktQC3CHA== new file mode 100644 index 0000000000..37a4d29e87 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bibbed-guinea-nRbpuKhwfQpLdUktQC3CHA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bitty-gibbon-6mCSsRY6uviFKCng2n02aw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bitty-gibbon-6mCSsRY6uviFKCng2n02aw== new file mode 100644 index 0000000000..c2f9bb5756 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bitty-gibbon-6mCSsRY6uviFKCng2n02aw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_blowy-puma-1UJ4.5aaOAwOhfweyVFhPw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_blowy-puma-1UJ4.5aaOAwOhfweyVFhPw== new file mode 100644 index 0000000000..3371d43233 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_blowy-puma-1UJ4.5aaOAwOhfweyVFhPw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_briny-beetle-oncLKIpDpBG4o3GoslH01g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_briny-beetle-oncLKIpDpBG4o3GoslH01g== new file mode 100644 index 0000000000..d3cbf8cfb0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_briny-beetle-oncLKIpDpBG4o3GoslH01g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_brunette-bee-cdmX7Za8g4NPT.Cn7NvXMg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_brunette-bee-cdmX7Za8g4NPT.Cn7NvXMg== new file mode 100644 index 0000000000..a92013670c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_brunette-bee-cdmX7Za8g4NPT.Cn7NvXMg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_built-hound-1zvU3N15NZg1tzdych6JEA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_built-hound-1zvU3N15NZg1tzdych6JEA== new file mode 100644 index 0000000000..fb642b2627 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_built-hound-1zvU3N15NZg1tzdych6JEA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bushed-finch-3323lzv.dQL+f7guSLFB0w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bushed-finch-3323lzv.dQL+f7guSLFB0w== new file mode 100644 index 0000000000..196dc36aaf Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_bushed-finch-3323lzv.dQL+f7guSLFB0w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_chafed-cougar-TEktJGT6YOQYnkDORxhuPA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_chafed-cougar-TEktJGT6YOQYnkDORxhuPA== new file mode 100644 index 0000000000..756d678f28 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_chafed-cougar-TEktJGT6YOQYnkDORxhuPA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cloggy-marten-Mvc9htpXXurpGwacJA46Tw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cloggy-marten-Mvc9htpXXurpGwacJA46Tw== new file mode 100644 index 0000000000..2b7e3d519f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cloggy-marten-Mvc9htpXXurpGwacJA46Tw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cruddy-clam-rT8uDbXCtU.FGyuXNTf8sw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cruddy-clam-rT8uDbXCtU.FGyuXNTf8sw== new file mode 100644 index 0000000000..a7bbad351c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cruddy-clam-rT8uDbXCtU.FGyuXNTf8sw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cubic-bat-pwD1EkIk6a1w9aQMszBxQA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cubic-bat-pwD1EkIk6a1w9aQMszBxQA== new file mode 100644 index 0000000000..0065f908a6 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_cubic-bat-pwD1EkIk6a1w9aQMszBxQA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_curly-taipan-mx0Ycf2US0u2jkhsDDK2Zg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_curly-taipan-mx0Ycf2US0u2jkhsDDK2Zg== new file mode 100644 index 0000000000..b5b6931571 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_curly-taipan-mx0Ycf2US0u2jkhsDDK2Zg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_curtal-bunny-3qe7SQGj.T9w+BtqYmiozg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_curtal-bunny-3qe7SQGj.T9w+BtqYmiozg== new file mode 100644 index 0000000000..7175261b64 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_curtal-bunny-3qe7SQGj.T9w+BtqYmiozg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_deft-dugong-9xcMwaMhQWlsoMNxLPAC2A== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_deft-dugong-9xcMwaMhQWlsoMNxLPAC2A== new file mode 100644 index 0000000000..42aff01c8c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_deft-dugong-9xcMwaMhQWlsoMNxLPAC2A== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_dosing-rodent-uJwkRUE2W9NRsQ+nSJLhDA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_dosing-rodent-uJwkRUE2W9NRsQ+nSJLhDA== new file mode 100644 index 0000000000..af537e56f2 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_dosing-rodent-uJwkRUE2W9NRsQ+nSJLhDA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_faced-turkey-zZVNxHDIk.mGyc2EGgM.Eg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_faced-turkey-zZVNxHDIk.mGyc2EGgM.Eg== new file mode 100644 index 0000000000..1135d3f65c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_faced-turkey-zZVNxHDIk.mGyc2EGgM.Eg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fifth-drone-xUUvkEG8vU2HzWnjpEpQfw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fifth-drone-xUUvkEG8vU2HzWnjpEpQfw== new file mode 100644 index 0000000000..8077f7c419 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fifth-drone-xUUvkEG8vU2HzWnjpEpQfw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_flint-locust-GshluIjQhVxK4JUfPErtaw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_flint-locust-GshluIjQhVxK4JUfPErtaw== new file mode 100644 index 0000000000..4eb2b1f63b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_flint-locust-GshluIjQhVxK4JUfPErtaw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fluffy-equid-MUb.NOcU+rJnf7n8g9iexw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fluffy-equid-MUb.NOcU+rJnf7n8g9iexw== new file mode 100644 index 0000000000..047af10809 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fluffy-equid-MUb.NOcU+rJnf7n8g9iexw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fly-pigeon-lOwkbamuKGghj34enJl.Sw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fly-pigeon-lOwkbamuKGghj34enJl.Sw== new file mode 100644 index 0000000000..b188dbdee2 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fly-pigeon-lOwkbamuKGghj34enJl.Sw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fried-lynx-0t9yCayIaVGG45a5FXr5FA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fried-lynx-0t9yCayIaVGG45a5FXr5FA== new file mode 100644 index 0000000000..ef7ae07cc0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fried-lynx-0t9yCayIaVGG45a5FXr5FA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fringy-lion-MwOvdtqLRe5kat.I8yIYpA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fringy-lion-MwOvdtqLRe5kat.I8yIYpA== new file mode 100644 index 0000000000..a657d48cb1 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fringy-lion-MwOvdtqLRe5kat.I8yIYpA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fringy-tomcat-HaWuncS4kZ29RPU2oT0P9w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fringy-tomcat-HaWuncS4kZ29RPU2oT0P9w== new file mode 100644 index 0000000000..97745f489b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fringy-tomcat-HaWuncS4kZ29RPU2oT0P9w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fuggy-marmot-BreO20z+UPVSdqaut0Xd0g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fuggy-marmot-BreO20z+UPVSdqaut0Xd0g== new file mode 100644 index 0000000000..48bf0493c1 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fuggy-marmot-BreO20z+UPVSdqaut0Xd0g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fuzzed-budgie-c8ivXTgHTPDfBJHY7biseg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fuzzed-budgie-c8ivXTgHTPDfBJHY7biseg== new file mode 100644 index 0000000000..fe690f3ffc Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_fuzzed-budgie-c8ivXTgHTPDfBJHY7biseg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_glace-anole-6cDgd5BqmQGV+otf40UljQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_glace-anole-6cDgd5BqmQGV+otf40UljQ== new file mode 100644 index 0000000000..fccb0676e5 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_glace-anole-6cDgd5BqmQGV+otf40UljQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_glued-hybrid-956kf9+vWJrfky+phpo++w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_glued-hybrid-956kf9+vWJrfky+phpo++w== new file mode 100644 index 0000000000..d3b92272fb Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_glued-hybrid-956kf9+vWJrfky+phpo++w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_goodly-bobcat-Agj7KBLhoeoMwoQ0AzCKTw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_goodly-bobcat-Agj7KBLhoeoMwoQ0AzCKTw== new file mode 100644 index 0000000000..c5bb72153c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_goodly-bobcat-Agj7KBLhoeoMwoQ0AzCKTw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_goodly-crow-v94j3qkZzKXvGTdZEmSKMA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_goodly-crow-v94j3qkZzKXvGTdZEmSKMA== new file mode 100644 index 0000000000..68c1ba19b2 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_goodly-crow-v94j3qkZzKXvGTdZEmSKMA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_grainy-gaur-Z2ch8cR76vieEvS3sl7wxQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_grainy-gaur-Z2ch8cR76vieEvS3sl7wxQ== new file mode 100644 index 0000000000..715f3fbe70 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_grainy-gaur-Z2ch8cR76vieEvS3sl7wxQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_grainy-skate-DZ.26yzPfzDR3Ftx2tOl9w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_grainy-skate-DZ.26yzPfzDR3Ftx2tOl9w== new file mode 100644 index 0000000000..fb51053f1e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_grainy-skate-DZ.26yzPfzDR3Ftx2tOl9w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_hated-worm-MtWGuJDTWqrty8+LbzEF3Q== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_hated-worm-MtWGuJDTWqrty8+LbzEF3Q== new file mode 100644 index 0000000000..30769aa68a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_hated-worm-MtWGuJDTWqrty8+LbzEF3Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_hulky-otus-mG.zxZ.wTgEy.x70tozyKA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_hulky-otus-mG.zxZ.wTgEy.x70tozyKA== new file mode 100644 index 0000000000..b52692e214 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_hulky-otus-mG.zxZ.wTgEy.x70tozyKA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jelled-lion-6pe0LDjXNahd1QZuZoM9XQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jelled-lion-6pe0LDjXNahd1QZuZoM9XQ== new file mode 100644 index 0000000000..482cf9189e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jelled-lion-6pe0LDjXNahd1QZuZoM9XQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jinxed-auk-64V0hkPm6dXlSEk5b8tyKQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jinxed-auk-64V0hkPm6dXlSEk5b8tyKQ== new file mode 100644 index 0000000000..bb2d9a877e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jinxed-auk-64V0hkPm6dXlSEk5b8tyKQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jovial-boxer-VOkd4Yg4duA8j4G15RpSiQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jovial-boxer-VOkd4Yg4duA8j4G15RpSiQ== new file mode 100644 index 0000000000..fad087811f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_jovial-boxer-VOkd4Yg4duA8j4G15RpSiQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_kookie-cobra-pRgcml1CaL103CjsqxkmnQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_kookie-cobra-pRgcml1CaL103CjsqxkmnQ== new file mode 100644 index 0000000000..52a36b31e9 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_kookie-cobra-pRgcml1CaL103CjsqxkmnQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_landed-egg-4KO4Dq24LTb5qW13g3wokg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_landed-egg-4KO4Dq24LTb5qW13g3wokg== new file mode 100644 index 0000000000..43ad5636d4 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_landed-egg-4KO4Dq24LTb5qW13g3wokg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_largo-goby-7lnEOV0K2OsQ15jOm.C43w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_largo-goby-7lnEOV0K2OsQ15jOm.C43w== new file mode 100644 index 0000000000..52f8d7434a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_largo-goby-7lnEOV0K2OsQ15jOm.C43w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_lentic-conger-CBbFn2oiox9JGifv4qYoLw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_lentic-conger-CBbFn2oiox9JGifv4qYoLw== new file mode 100644 index 0000000000..d1da623030 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_lentic-conger-CBbFn2oiox9JGifv4qYoLw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_lunate-oxen-hRQVVLQxL1DDKvX1tLZz4w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_lunate-oxen-hRQVVLQxL1DDKvX1tLZz4w== new file mode 100644 index 0000000000..f50b9f624a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_lunate-oxen-hRQVVLQxL1DDKvX1tLZz4w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_macho-fleece-fiYWw+6akEm5yc9SIHAurQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_macho-fleece-fiYWw+6akEm5yc9SIHAurQ== new file mode 100644 index 0000000000..56cf4514c8 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_macho-fleece-fiYWw+6akEm5yc9SIHAurQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_manic-kelp-v9D8TPVkPwEHr4pQIlwBIw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_manic-kelp-v9D8TPVkPwEHr4pQIlwBIw== new file mode 100644 index 0000000000..dfad029b99 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_manic-kelp-v9D8TPVkPwEHr4pQIlwBIw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_miffed-grouse-CJH3MjtV8+Rwq3oSqiKlXQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_miffed-grouse-CJH3MjtV8+Rwq3oSqiKlXQ== new file mode 100644 index 0000000000..f483d33825 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_miffed-grouse-CJH3MjtV8+Rwq3oSqiKlXQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_milky-ibex-9u4FMRV0ZldZlSv9efOueg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_milky-ibex-9u4FMRV0ZldZlSv9efOueg== new file mode 100644 index 0000000000..be61144b39 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_milky-ibex-9u4FMRV0ZldZlSv9efOueg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_milled-liger-4Xu6dzzgWTBt0nVuErBX3w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_milled-liger-4Xu6dzzgWTBt0nVuErBX3w== new file mode 100644 index 0000000000..4d3d271e6c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_milled-liger-4Xu6dzzgWTBt0nVuErBX3w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_napped-mare-YAFpPMbD0.2y0fMhjVjH9w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_napped-mare-YAFpPMbD0.2y0fMhjVjH9w== new file mode 100644 index 0000000000..a257b0c4fe Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_napped-mare-YAFpPMbD0.2y0fMhjVjH9w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nasal-moose-ZtqSbv2rFpsNt4FQrgWvew== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nasal-moose-ZtqSbv2rFpsNt4FQrgWvew== new file mode 100644 index 0000000000..9ef7b66783 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nasal-moose-ZtqSbv2rFpsNt4FQrgWvew== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_niffy-mayfly-1HJyCdNm.zuH0GlC+j52Yw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_niffy-mayfly-1HJyCdNm.zuH0GlC+j52Yw== new file mode 100644 index 0000000000..442255e88b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_niffy-mayfly-1HJyCdNm.zuH0GlC+j52Yw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nonfat-trout-WnCtapADz+Cf7bqO2nsSgg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nonfat-trout-WnCtapADz+Cf7bqO2nsSgg== new file mode 100644 index 0000000000..78b286e990 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nonfat-trout-WnCtapADz+Cf7bqO2nsSgg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nordic-pike-vHPyU1Uaq8Ia8MQfjXkrIg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nordic-pike-vHPyU1Uaq8Ia8MQfjXkrIg== new file mode 100644 index 0000000000..dace299e29 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_nordic-pike-vHPyU1Uaq8Ia8MQfjXkrIg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_passe-okapi-5ui2bLBiHINZDqCzyDHC9g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_passe-okapi-5ui2bLBiHINZDqCzyDHC9g== new file mode 100644 index 0000000000..32abe7ec97 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_passe-okapi-5ui2bLBiHINZDqCzyDHC9g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_paved-badger-Fb6IyHMz.evdUUP3Cc9CUg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_paved-badger-Fb6IyHMz.evdUUP3Cc9CUg== new file mode 100644 index 0000000000..fc41f0b53d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_paved-badger-Fb6IyHMz.evdUUP3Cc9CUg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_peanut-puppy-GujXTTeQA.keohfe93ZCFA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_peanut-puppy-GujXTTeQA.keohfe93ZCFA== new file mode 100644 index 0000000000..20b634b908 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_peanut-puppy-GujXTTeQA.keohfe93ZCFA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_piano-slug-t4CuKTKx7qD6.ogOO3KY8A== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_piano-slug-t4CuKTKx7qD6.ogOO3KY8A== new file mode 100644 index 0000000000..dd6d20ed67 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_piano-slug-t4CuKTKx7qD6.ogOO3KY8A== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_planar-badger-1CJJt3UpWnkka7Hq.l4f4g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_planar-badger-1CJJt3UpWnkka7Hq.l4f4g== new file mode 100644 index 0000000000..417f9137a8 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_planar-badger-1CJJt3UpWnkka7Hq.l4f4g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_planar-kelp-6RdIIoJglGHAA2ZivPSwvQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_planar-kelp-6RdIIoJglGHAA2ZivPSwvQ== new file mode 100644 index 0000000000..c3f4808c8b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_planar-kelp-6RdIIoJglGHAA2ZivPSwvQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pricey-bronco-EMUXbq1YtlHzi943TJpZug== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pricey-bronco-EMUXbq1YtlHzi943TJpZug== new file mode 100644 index 0000000000..3e056dba96 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pricey-bronco-EMUXbq1YtlHzi943TJpZug== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_primed-gibbon-ibHcfWAKIQ4RUPTdUgb.Ag== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_primed-gibbon-ibHcfWAKIQ4RUPTdUgb.Ag== new file mode 100644 index 0000000000..32c1ab1d18 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_primed-gibbon-ibHcfWAKIQ4RUPTdUgb.Ag== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pudgy-walrus-6s0BO2a6d+Jjrp43oZRKTA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pudgy-walrus-6s0BO2a6d+Jjrp43oZRKTA== new file mode 100644 index 0000000000..a0f34c321f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pudgy-walrus-6s0BO2a6d+Jjrp43oZRKTA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pyknic-maggot-XdBmIKBhNeDzQRAsmzuTMQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pyknic-maggot-XdBmIKBhNeDzQRAsmzuTMQ== new file mode 100644 index 0000000000..829b6224dd Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_pyknic-maggot-XdBmIKBhNeDzQRAsmzuTMQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_random-monkey-TAgyE15e7iJAOClZ2QL4xA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_random-monkey-TAgyE15e7iJAOClZ2QL4xA== new file mode 100644 index 0000000000..05b3238627 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_random-monkey-TAgyE15e7iJAOClZ2QL4xA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_reefy-merlin-qNMU4vTDVfemhU0O7CpUig== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_reefy-merlin-qNMU4vTDVfemhU0O7CpUig== new file mode 100644 index 0000000000..d34043e179 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_reefy-merlin-qNMU4vTDVfemhU0O7CpUig== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roan-mole-7hAoYalgglrWq5MlfVRzQA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roan-mole-7hAoYalgglrWq5MlfVRzQA== new file mode 100644 index 0000000000..0ab4b1ee36 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roan-mole-7hAoYalgglrWq5MlfVRzQA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roast-pincer-tUZvKSROlgKsyM6M6rP0ew== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roast-pincer-tUZvKSROlgKsyM6M6rP0ew== new file mode 100644 index 0000000000..3d0a49c9d7 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roast-pincer-tUZvKSROlgKsyM6M6rP0ew== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_robed-weevil-x10o3LK4K1ggqc7ngH0ryQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_robed-weevil-x10o3LK4K1ggqc7ngH0ryQ== new file mode 100644 index 0000000000..855c4c0e2e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_robed-weevil-x10o3LK4K1ggqc7ngH0ryQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roomy-tiger-CG8vW8lZfVXqameuEl5XsA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roomy-tiger-CG8vW8lZfVXqameuEl5XsA== new file mode 100644 index 0000000000..e134a92297 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_roomy-tiger-CG8vW8lZfVXqameuEl5XsA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rotted-grub-dFgKsrLHkAU8zjbe+xwP4w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rotted-grub-dFgKsrLHkAU8zjbe+xwP4w== new file mode 100644 index 0000000000..75f3b42186 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rotted-grub-dFgKsrLHkAU8zjbe+xwP4w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rowdy-mule-24+2yyNxy59S5fy999Vzzw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rowdy-mule-24+2yyNxy59S5fy999Vzzw== new file mode 100644 index 0000000000..e53f0efb9b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rowdy-mule-24+2yyNxy59S5fy999Vzzw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rushy-salmon-ytrRtyoUOQXp.b2.NkqelQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rushy-salmon-ytrRtyoUOQXp.b2.NkqelQ== new file mode 100644 index 0000000000..4aa42e32e3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rushy-salmon-ytrRtyoUOQXp.b2.NkqelQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rushy-vulture-acz1Cgl9on+YKgJSnBPFYA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rushy-vulture-acz1Cgl9on+YKgJSnBPFYA== new file mode 100644 index 0000000000..d05335042f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rushy-vulture-acz1Cgl9on+YKgJSnBPFYA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rust-warble-BxQCrgkRekJ1kEBOZgvK+A== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rust-warble-BxQCrgkRekJ1kEBOZgvK+A== new file mode 100644 index 0000000000..58653132a3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_rust-warble-BxQCrgkRekJ1kEBOZgvK+A== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_scenic-eagle-FIz7b+TCtAYsLdGjxEHvGA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_scenic-eagle-FIz7b+TCtAYsLdGjxEHvGA== new file mode 100644 index 0000000000..f77334d9df Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_scenic-eagle-FIz7b+TCtAYsLdGjxEHvGA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_sedgy-mudcat-riO1fZ2JUHgQqty9cDUFlQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_sedgy-mudcat-riO1fZ2JUHgQqty9cDUFlQ== new file mode 100644 index 0000000000..921973d589 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_sedgy-mudcat-riO1fZ2JUHgQqty9cDUFlQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_shaken-dassie-dTgl5py1U8zMgEFWIMovog== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_shaken-dassie-dTgl5py1U8zMgEFWIMovog== new file mode 100644 index 0000000000..3c0f3304b7 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_shaken-dassie-dTgl5py1U8zMgEFWIMovog== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_shorn-crest-iiJerZBz7.DgPuL8gRTp8g== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_shorn-crest-iiJerZBz7.DgPuL8gRTp8g== new file mode 100644 index 0000000000..2d2d3684cb Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_shorn-crest-iiJerZBz7.DgPuL8gRTp8g== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_slimed-mantis-abJL1rDjRwzfB93qlk0VEw== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_slimed-mantis-abJL1rDjRwzfB93qlk0VEw== new file mode 100644 index 0000000000..b2027a15f2 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_slimed-mantis-abJL1rDjRwzfB93qlk0VEw== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_sly-ewe-ecXKNfiC43RHOq1iGgiaSg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_sly-ewe-ecXKNfiC43RHOq1iGgiaSg== new file mode 100644 index 0000000000..22eaa09afd Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_sly-ewe-ecXKNfiC43RHOq1iGgiaSg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_smug-crow-xk8dYqvVx7dB2PFpGAO80w== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_smug-crow-xk8dYqvVx7dB2PFpGAO80w== new file mode 100644 index 0000000000..e51eb0075e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_smug-crow-xk8dYqvVx7dB2PFpGAO80w== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_staid-auk-5lm0kPBGCLWIBxmM9zyiPA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_staid-auk-5lm0kPBGCLWIBxmM9zyiPA== new file mode 100644 index 0000000000..0a81ba0c93 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_staid-auk-5lm0kPBGCLWIBxmM9zyiPA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_starry-spawn-+1MuXNIAYJD7GJnqOqoBaA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_starry-spawn-+1MuXNIAYJD7GJnqOqoBaA== new file mode 100644 index 0000000000..46ca7e0e7c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_starry-spawn-+1MuXNIAYJD7GJnqOqoBaA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_stocky-bobcat-eS+RBBa4hDJwD16ak3.rog== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_stocky-bobcat-eS+RBBa4hDJwD16ak3.rog== new file mode 100644 index 0000000000..397653eab7 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_stocky-bobcat-eS+RBBa4hDJwD16ak3.rog== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_stubby-canary-2AHogR9tmE+FDSluowyecA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_stubby-canary-2AHogR9tmE+FDSluowyecA== new file mode 100644 index 0000000000..572bdd760e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_stubby-canary-2AHogR9tmE+FDSluowyecA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_taxing-donkey-r1GaN5aWQ3tmnsfZtAX8iQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_taxing-donkey-r1GaN5aWQ3tmnsfZtAX8iQ== new file mode 100644 index 0000000000..1075708fcd Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_taxing-donkey-r1GaN5aWQ3tmnsfZtAX8iQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_twee-botfly-bfft3rV06MRa4kX326WSjQ== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_twee-botfly-bfft3rV06MRa4kX326WSjQ== new file mode 100644 index 0000000000..ab0acc88e4 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_twee-botfly-bfft3rV06MRa4kX326WSjQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_umteen-puffer-bE6FiOu.74LOKt.4Hvn2CA== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_umteen-puffer-bE6FiOu.74LOKt.4Hvn2CA== new file mode 100644 index 0000000000..82f201cb9d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_umteen-puffer-bE6FiOu.74LOKt.4Hvn2CA== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_upland-gnu-CJLPRnnlI4MKX9QuDsezJg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_upland-gnu-CJLPRnnlI4MKX9QuDsezJg== new file mode 100644 index 0000000000..b312c9ee84 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_upland-gnu-CJLPRnnlI4MKX9QuDsezJg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_worst-ermine-EihtDFP82nMOtWaJQFPSHg== b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_worst-ermine-EihtDFP82nMOtWaJQFPSHg== new file mode 100644 index 0000000000..a1e91d24e2 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/b45-621-cleanup/waypoint_snapshots/snapshot_worst-ermine-EihtDFP82nMOtWaJQFPSHg== differ diff --git a/predicators/spot_utils/graph_nav_maps/b45-621/metadata.yaml b/predicators/spot_utils/graph_nav_maps/b45-621/metadata.yaml index cb37e2d951..5a589198a2 100644 --- a/predicators/spot_utils/graph_nav_maps/b45-621/metadata.yaml +++ b/predicators/spot_utils/graph_nav_maps/b45-621/metadata.yaml @@ -1,16 +1,21 @@ # Additional info associated with the map. --- # NOTE: close to the center of the room -#spot-home-pose: -# x: 2.85 -# y: 0.0 -# angle: -1.45769 -# NOTE: a place closer to the corner that the cable can reach - easier for debug spot-home-pose: - x: 3.5 - y: 0.45 - angle: 0. -april-tag-offsets: [] + x: 2.85 + y: 0.0 + angle: 3.14 #1.45769 +# NOTE: a place closer to the corner that the cable can reach - easier for debug +# spot-home-pose: +# x: 3.5 +# y: 0.45 +# angle: 0. +# april-tag-offsets: [] +# NOTE: close to the door of the room +# spot-home-pose: +# x: 4.25 +# y: -0.7 +# angle: 3.14 # Allowed regions. Each region is defined by a set of points # that form the boundary of the region. We will check @@ -19,9 +24,9 @@ april-tag-offsets: [] # points. allowed-regions: spot-room: - - [0.25, 1.9] - - [1.2, -2.5] - - [5.7, -1.5] + - [0.0, 1.9] + - [0.2, -2.5] + - [5.0, -2.5] - [5.0, 2.7] # Known immovable objects. Assuming default rotations. @@ -31,6 +36,30 @@ known-immovable-objects: x: 1.4 y: 0.5 z: -0.5 + wooden_table: + x: 0.7 + y: -0.7 + z: -0.4 + child_play_table: + x: 1.65 + y: -0.24 + z: 0.14 + +# Helpful for static objects that are up against a wall, for example. +approach_angle_bounds: + wooden_table: [-0.1, 0.1] # about -pi / 2 + # Only drag the chair from behind it, approximately. + blue_toy_chair: [-0.1, 0.1] #[-1.75, -1.25] # about -pi / 2 + # Only approach the brush from perpendicular to it! + brush: [1.25, 1.75] + floor: [-0.1, 0.1] + bucket: [-0.1, 0.1] + # For spot_vlm_table_wiping_invention_env + child_play_table: [-0.1, 0.1] + apple: [-0.1, 0.1] + clear_plastic_dustbin: [1.47, 1.67] + cardboard_box_bin: [1.47, 1.67] + fluffy_green_toy_eraser: [-0.1, 0.1] # Static object features, including the shapes and sizes of known objects. static-object-features: @@ -46,4 +75,249 @@ static-object-features: length: 0.1 width: 0.1 placeable: 1 - is_sweeper: 0 \ No newline at end of file + is_sweeper: 0 + blue_block: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + green_handle: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 0 + is_sweeper: 0 + yellow_cup: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + toy_plane: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + cardboard_box: + shape: 1 + height: 0.05 + width: 0.3 + length: 0.4 + placeable: 1 + is_sweeper: 0 + # Important for TopAbove that the bucket appears to always be on the ground + z: -0.15 + # Rotation can't be detected. + qw: 1 + qx: 0 + qy: 0 + qz: 0 + tennis_ball: + shape: 2 + height: 0.067 + length: 0.067 + width: 0.067 + placeable: 1 + is_sweeper: 0 + red_ball: + shape: 2 + height: 0.05 + length: 0.05 + width: 0.05 + placeable: 1 + is_sweeper: 0 + yellow_table: + shape: 1 + height: 0.75 + length: 1.0 + width: 0.6 + flat_top_surface: 1 + wooden_table: + shape: 1 + height: 0.9 + length: 1.5 + width: 1.5 + flat_top_surface: 1 + brown_bear_toy: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + panda_toy: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + chick_toy: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + blue_toy_chair: + shape: 1 + height: 0.2 + length: 0.5 + width: 0.5 + placeable: 0 + is_sweeper: 0 + brush: + shape: 1 + height: 0.05 + length: 0.5 + width: 0.05 + placeable: 1 + is_sweeper: 1 + bucket: + shape: 1 + height: 0.05 + width: 0.4 + length: 0.5 + placeable: 1 + is_sweeper: 0 + # Important for TopAbove that the bucket appears to always be on the ground + z: -0.45 + # Rotation can't be detected. + qw: 1 + qx: 0 + qy: 0 + qz: 0 + orange_bucket: + shape: 1 + height: 0.3 + width: 0.3 + length: 0.3 + placeable: 1 + is_sweeper: 0 + z: -0.35 + qw: 1 + qx: 0 + qy: 0 + qz: 0 + sponge: + shape: 1 + height: 0.05 + length: 0.12 + width: 0.08 + placeable: 1 + is_sweeper: 1 + yellow_toy_cup: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + small_cardboard_box_with_black_tape: + shape: 1 # cuboid + height: 0.55 + length: 0.28 + width: 0.6 + flat_top_surface: 1 + apple: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + clear_plastic_trash_can: + shape: 2 + height: 0.1 + length: 0.2 + width: 0.2 + placeable: 1 + is_sweeper: 0 + flat_top_surface: 0 + fluffy_toy_duster: + shape: 1 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + table: + shape: 1 + height: 0.1 + length: 0.3 + width: 0.5 + flat_top_surface: 1 + # Objects for spot_vlm_table_wiping_invention_env + child_play_table: + shape: 1 + height: 0.1 + length: 0.79 + width: 0.62 + flat_top_surface: 1 + green_block: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + orange_block: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + spam_tin: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + clear_plastic_dustbin: + shape: 2 + height: 0.1 + length: 0.2 + width: 0.2 + placeable: 1 + is_sweeper: 0 + flat_top_surface: 0 + fluffy_green_toy_eraser: + shape: 1 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + red_drink_cup: + shape: 2 + height: 0.1 + length: 0.1 + width: 0.1 + placeable: 1 + is_sweeper: 0 + cardboard_box_bin: + shape: 2 + height: 0.1 + length: 0.2 + width: 0.2 + placeable: 1 + is_sweeper: 0 + flat_top_surface: 0 +# This should be calculable, but I'm lazy. These values represent the SE2 pose +# (in the table frame) for the robot before placing the container. +prepare_container_relative_xy: + dx: -0.25 + dy: 0.75 + angle: -1.5707 # - pi / 2 + +# Wiping skill params. +wipe_location: + child_play_table: [2.48, 0.205, -2.96856] \ No newline at end of file diff --git a/predicators/spot_utils/perception/object_detection.py b/predicators/spot_utils/perception/object_detection.py index 388cf61845..b1d9235205 100644 --- a/predicators/spot_utils/perception/object_detection.py +++ b/predicators/spot_utils/perception/object_detection.py @@ -244,6 +244,7 @@ def _get_detection_score(img_detections: Dict[str, SegmentedBoundingBox], f"{obj_id} because it's out of bounds. " + \ f"(pose = {pose_xy})") continue + # Pose extraction succeeded. detections[obj_id] = pose break @@ -472,7 +473,7 @@ def get_random_mask_pixel_from_artifacts( pixels_in_mask = np.where(mask) mask_idx = rng.choice(len(pixels_in_mask)) pixel_tuple = (pixels_in_mask[1][mask_idx], pixels_in_mask[0][mask_idx]) - # Uncomment to plot the grasp pixel being selected! + # # Uncomment to plot the grasp pixel being selected! # rgb_img = artifacts["language"]["rgbds"][camera_name].rgb # _, axes = plt.subplots() # axes.imshow(rgb_img) diff --git a/predicators/spot_utils/perception/object_specific_grasp_selection.py b/predicators/spot_utils/perception/object_specific_grasp_selection.py index db975a397d..fc344f8652 100644 --- a/predicators/spot_utils/perception/object_specific_grasp_selection.py +++ b/predicators/spot_utils/perception/object_specific_grasp_selection.py @@ -20,15 +20,18 @@ "white button" ]) ball_obj = LanguageObjectDetectionID(ball_prompt) +apple_obj = LanguageObjectDetectionID("apple/red_ball") cup_obj = LanguageObjectDetectionID("yellow hoop toy/yellow donut") brush_prompt = "/".join( - ["scrubbing brush", "hammer", "mop", "giant white toothbrush"]) + ["scrubbing brush", "hammer", "mop", "giant white toothbrush", "squeegee", "white broom head", "white brush"]) brush_obj = LanguageObjectDetectionID(brush_prompt) bucket_prompt = "/".join([ "white plastic container with black handles", "white plastic tray with black handles", "white plastic bowl", "white storage bin with black handles", + "small grey container", #TODO + "white container with toys inside" #TODO ]) bucket_obj = LanguageObjectDetectionID(bucket_prompt) football_prompt = "/".join(["small orange basketball", "small orange"]) @@ -41,6 +44,15 @@ train_toy_obj = LanguageObjectDetectionID(train_toy_prompt) chair_prompt = "chair" chair_obj = LanguageObjectDetectionID(chair_prompt) +blue_toy_chair_prompt = "blue toy chair" +blue_toy_chair_obj = LanguageObjectDetectionID(blue_toy_chair_prompt) +orange_bucket_prompt = "/".join([ + "orange bucket", "orange Home Depot bucket", "orange plastic bucket" +]) +orange_bucket_obj = LanguageObjectDetectionID(orange_bucket_prompt) +trash_can_obj = LanguageObjectDetectionID("bottle/clear_cup/clear_trashcan") +blue_cup_obj = LanguageObjectDetectionID("blue_coffee_cup") +eraser_obj = LanguageObjectDetectionID("fluffy_toy/flower_arrangement") def _get_platform_grasp_pixel( @@ -94,6 +106,32 @@ def _get_ball_grasp_pixel( return pixel, pitch * roll # NOTE: order is super important here! +def _get_apple_grasp_pixel( + rgbds: Dict[str, RGBDImageWithContext], artifacts: Dict[str, Any], + camera_name: str, rng: np.random.Generator +) -> Tuple[Tuple[int, int], Optional[math_helpers.Quat]]: + # del rgbds, rng + detections = artifacts["language"]["object_id_to_img_detections"] + try: + seg_bb = detections[apple_obj][camera_name] + except KeyError: + raise ValueError(f"{apple_obj} not detected in {camera_name}") + mask = seg_bb.mask + pixels_in_mask = np.where(mask) + # Select a pixel near the bottom, but not exactly the bottom-most. + pixel = (pixels_in_mask[1][-1], pixels_in_mask[0][-400]) + # Force a forward top-down grasp. + roll = math_helpers.Quat.from_roll(np.pi / 2) + pitch = math_helpers.Quat.from_pitch(np.pi / 2) + # # # Uncomment for debugging. + # bgr = cv2.cvtColor(rgbds[camera_name].rgb, cv2.COLOR_RGB2BGR) + # cv2.circle(bgr, pixel, 5, (0, 255, 0), -1) + # cv2.imshow("Selected grasp", bgr) + # cv2.waitKey(0) + # cv2.destroyAllWindows() + return pixel, pitch * roll # NOTE: order is super important here! + + def _get_chair_grasp_pixel( rgbds: Dict[str, RGBDImageWithContext], artifacts: Dict[str, Any], camera_name: str, rng: np.random.Generator @@ -147,6 +185,36 @@ def _get_chair_grasp_pixel( pitch = math_helpers.Quat.from_pitch(np.pi / 2) return pixel, pitch +def _get_blue_toy_chair_grasp_pixel( + rgbds: Dict[str, RGBDImageWithContext], artifacts: Dict[str, Any], + camera_name: str, rng: np.random.Generator +) -> Tuple[Tuple[int, int], Optional[math_helpers.Quat]]: + del rng + detections = artifacts["language"]["object_id_to_img_detections"] + try: + seg_bb = detections[blue_toy_chair_obj][camera_name] + except KeyError: + raise ValueError(f"{blue_toy_chair_obj} not detected in {camera_name}") + mask = seg_bb.mask + rgbd = rgbds[camera_name] + pixels_in_mask = np.where(mask) + rows, cols = pixels_in_mask + top_2_mask = rows <= rows.min() + 0.2 * (rows.max() - rows.min()) + pixel_tuple = (int(np.median(cols[top_2_mask])), int(rows[top_2_mask].min())) + print("DEBUG: using top 20% of chair mask for grasp pixel selection", pixel_tuple) + + # Uncomment for debugging. + # rgbd = rgbds[camera_name] + # bgr = cv2.cvtColor(rgbd.rgb, cv2.COLOR_RGB2BGR) + # cv2.circle(bgr, pixel, 5, (0, 255, 0), -1) + # cv2.circle(bgr, pixel, 5, (255, 0, 0), -1) + # cv2.imshow("Selected grasp", bgr) + # cv2.waitKey(0) + # cv2.destroyAllWindows() + + # Force a top-down grasp. + pitch = math_helpers.Quat.from_pitch(np.pi / 2) + return pixel_tuple, pitch def _get_cup_grasp_pixel( rgbds: Dict[str, RGBDImageWithContext], artifacts: Dict[str, Any], @@ -250,7 +318,7 @@ def _get_brush_grasp_pixel( mask = (convolved_mask > 0) # Get copy of image with just the mask pixels in it. isolated_rgb = rgb.copy() - isolated_rgb[~mask] = 0 + # isolated_rgb[~mask] = 0 # TODO use whole image # Look for blue pixels in the isolated rgb. lo, hi = ((0, 130, 180), (130, 255, 255)) centroid = find_color_based_centroid(isolated_rgb, @@ -410,6 +478,138 @@ def _get_bucket_grasp_pixel( return selected_pixel, pitch +def _get_orange_bucket_grasp_pixel( + rgbds: Dict[str, RGBDImageWithContext], artifacts: Dict[str, Any], + camera_name: str, rng: np.random.Generator +) -> Tuple[Tuple[int, int], Optional[math_helpers.Quat]]: + """Select a blue pixel on the rim of the bucket to grasp.""" + del rng # not used + + detections = artifacts["language"]["object_id_to_img_detections"] + try: + seg_bb = detections[orange_bucket_obj][camera_name] + except KeyError: + raise ValueError(f"{orange_bucket_obj} not detected in {camera_name}") + + mask = seg_bb.mask + rgbd = rgbds[camera_name] + + # Helpful to dump these things and analyze separately. + # import dill as pkl + # with open("debug.pkl", "wb") as f: + # pkl.dump( + # { + # "rgbd": rgbd, + # "mask": mask, + # }, f) + + # Look for blue pixels in the isolated rgb. + # Start by denoising the mask, "filling in" small gaps in it. + convolved_mask = convolve(mask.astype(np.uint8), + np.ones((3, 3)), + mode="constant") + smoothed_mask = (convolved_mask > 0) + # Get copy of image with just the mask pixels in it. + isolated_rgb = rgbd.rgb.copy() + isolated_rgb[~smoothed_mask] = 0 + lo, hi = ((0, 0, 130), (130, 255, 255)) + centroid = find_color_based_centroid(isolated_rgb, + lo, + hi, + min_component_size=10) + # This can happen sometimes if the rim of the bucket is separated from the + # body of the bucket. If that happens, just pick the center bottom pixel in + # the mask, which should be the rim. + if centroid is None: + mask_args = np.argwhere(mask) + mask_min_c = min(mask_args[:, 1]) + mask_max_c = max(mask_args[:, 1]) + c_len = mask_max_c - mask_min_c + middle_c = mask_min_c + c_len // 2 + max_r = max(r for r, c in mask_args if c == middle_c) + selected_pixel = (middle_c, max_r) + else: + # NOTE! Testing + selected_pixel = (centroid[0], centroid[1]) + + # Uncomment for debugging. + # bgr = cv2.cvtColor(rgbds[camera_name].rgb, cv2.COLOR_RGB2BGR) + # cv2.circle(bgr, selected_pixel, 5, (0, 255, 0), -1) + # cv2.imshow("Selected grasp", bgr) + # cv2.waitKey(0) + # cv2.destroyAllWindows() + + # Specify a top-down grasp constraint. + pitch = math_helpers.Quat.from_pitch(np.pi / 2) + + return selected_pixel, pitch + +def _get_trash_grasp_pixel( + rgbds: Dict[str, RGBDImageWithContext], artifacts: Dict[str, Any], + camera_name: str, rng: np.random.Generator +) -> Tuple[Tuple[int, int], Optional[math_helpers.Quat]]: + """Select a blue pixel on the rim of the bucket to grasp.""" + del rng # not used + + detections = artifacts["language"]["object_id_to_img_detections"] + try: + seg_bb = detections[trash_can_obj][camera_name] + except KeyError: + raise ValueError(f"{trash_can_obj} not detected in {camera_name}") + + mask = seg_bb.mask + rgbd = rgbds[camera_name] + + # Helpful to dump these things and analyze separately. + # import dill as pkl + # with open("debug.pkl", "wb") as f: + # pkl.dump( + # { + # "rgbd": rgbd, + # "mask": mask, + # }, f) + + # Look for blue pixels in the isolated rgb. + # Start by denoising the mask, "filling in" small gaps in it. + convolved_mask = convolve(mask.astype(np.uint8), + np.ones((3, 3)), + mode="constant") + smoothed_mask = (convolved_mask > 0) + # Get copy of image with just the mask pixels in it. + isolated_rgb = rgbd.rgb.copy() + isolated_rgb[~smoothed_mask] = 0 + lo, hi = ((0, 0, 130), (130, 255, 255)) + centroid = find_color_based_centroid(isolated_rgb, + lo, + hi, + min_component_size=10) + # This can happen sometimes if the rim of the bucket is separated from the + # body of the bucket. If that happens, just pick the center bottom pixel in + # the mask, which should be the rim. + if centroid is None: + mask_args = np.argwhere(mask) + mask_min_c = min(mask_args[:, 1]) + mask_max_c = max(mask_args[:, 1]) + c_len = mask_max_c - mask_min_c + middle_c = mask_min_c + c_len // 2 + max_r = max(r for r, c in mask_args if c == middle_c) + selected_pixel = (middle_c, max_r) + else: + # NOTE! Testing + selected_pixel = (centroid[0], centroid[1]) + + # # Uncomment for debugging. + # bgr = cv2.cvtColor(rgbds[camera_name].rgb, cv2.COLOR_RGB2BGR) + # cv2.circle(bgr, selected_pixel, 5, (0, 255, 0), -1) + # cv2.imshow("Selected grasp", bgr) + # cv2.waitKey(0) + # cv2.destroyAllWindows() + + # Specify a top-down grasp constraint. + pitch = math_helpers.Quat.from_pitch(np.pi / 2) + + return selected_pixel, pitch + def _get_mask_center_grasp_pixel( detect_id: LanguageObjectDetectionID, rgbds: Dict[str, @@ -453,6 +653,8 @@ def _get_mask_center_grasp_pixel( AprilTagObjectDetectionID(411): _get_platform_grasp_pixel, # Ball-specific grasp selection. ball_obj: _get_ball_grasp_pixel, + # Apple-specific grasp selection. + apple_obj: _get_apple_grasp_pixel, # Cup-specific grasp selection. cup_obj: _get_cup_grasp_pixel, # Brush-specific grasp selection. @@ -464,5 +666,15 @@ def _get_mask_center_grasp_pixel( # train_toy-specific grasp selection. train_toy_obj: partial(_get_mask_center_grasp_pixel, train_toy_obj), # Chair-specific grasp selection. - chair_obj: _get_chair_grasp_pixel + chair_obj: _get_chair_grasp_pixel, + # Blue toy chair-specific grasp selection. + blue_toy_chair_obj: _get_blue_toy_chair_grasp_pixel, + # Orange bucket-specific grasp selection. + orange_bucket_obj: _get_orange_bucket_grasp_pixel, + # Trash can specific grasp selection. + trash_can_obj: _get_trash_grasp_pixel, + # Blue cup specific grasp selection. + blue_cup_obj: partial(_get_mask_center_grasp_pixel, blue_cup_obj), + # Eraser-specific grasp selection. + eraser_obj: partial(_get_mask_center_grasp_pixel, eraser_obj), } diff --git a/predicators/spot_utils/skills/spot_dump.py b/predicators/spot_utils/skills/spot_dump.py index 474d9a7194..225cb10ded 100644 --- a/predicators/spot_utils/skills/spot_dump.py +++ b/predicators/spot_utils/skills/spot_dump.py @@ -23,15 +23,16 @@ def dump_container(robot: Robot, Assumes that the container is grasped with a top-down grasp on the side of the container, and with the fingers pointed inward. """ - # Construct the desired hand pose for dumping. - yaw = math_helpers.Quat.from_yaw(np.pi / 3) - pitch = math_helpers.Quat.from_roll(np.pi / 3) - roll = math_helpers.Quat.from_roll(np.pi / 3) - rot = roll * yaw * pitch + # # Construct the desired hand pose for dumping. + # yaw = math_helpers.Quat.from_yaw(np.pi / 3) + # roll2 = math_helpers.Quat.from_roll(np.pi / 3) + # roll1 = math_helpers.Quat.from_roll(np.pi / 3) + # rot = roll1 * yaw * roll2 + dump_quat = math_helpers.Quat(w=0.865, x=-0.201, y=-0.448, z=0.102) hand_dump_pose = math_helpers.SE3Pose(x=dump_x, y=dump_y, z=dump_z, - rot=rot) + rot=dump_quat) # Execute the move to the pose. move_hand_to_relative_pose(robot, hand_dump_pose) # Wait a few seconds for the object(s) to be dumped. @@ -41,6 +42,32 @@ def dump_container(robot: Robot, place_at_relative_position(robot, body_to_position, place_angle) +# def get_end_effector_state(robot) -> None: +# """Get the current position and orientation of the Spot arm's end effector.""" +# # Create a RobotStateClient to query the robot's state +# from bosdyn.client.robot_state import RobotStateClient + +# state_client = robot.ensure_client(RobotStateClient.default_service_name) + +# # Get the robot's state +# robot_state = state_client.get_robot_state() + +# # Access the kinematic state of the arm +# arm_state = robot_state.kinematic_state + +# # Extract the end-effector pose (position and orientation) +# if arm_state and arm_state.transforms_snapshot: +# ee_transform = arm_state.transforms_snapshot.child_to_parent_edge_map.get("hand", None) +# if ee_transform: +# position = ee_transform.parent_tform_child.position +# orientation = ee_transform.parent_tform_child.rotation +# print(f"End Effector Position: x={position.x}, y={position.y}, z={position.z}") +# print(f"End Effector Orientation (Quaternion): x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}") +# else: +# print("End effector transform not found.") +# else: +# print("Arm state or transforms snapshot not available.") + if __name__ == "__main__": # Run this file alone to test manually. # Make sure to pass in --spot_robot_ip. @@ -77,7 +104,7 @@ def _run_manual_test() -> None: hostname = CFG.spot_robot_ip path = get_graph_nav_dir() - sdk = create_standard_sdk('GraspSkillTestClient') + sdk = create_standard_sdk('DumpSkillTestClient') robot = sdk.create_robot(hostname) authenticate(robot) verify_estop(robot) @@ -111,5 +138,6 @@ def _run_manual_test() -> None: # Dump! dump_container(robot, place_height) + # get_end_effector_state(robot) _run_manual_test() diff --git a/predicators/spot_utils/skills/spot_find_objects.py b/predicators/spot_utils/skills/spot_find_objects.py index ce8db3826d..43c42eb7d1 100644 --- a/predicators/spot_utils/skills/spot_find_objects.py +++ b/predicators/spot_utils/skills/spot_find_objects.py @@ -97,12 +97,12 @@ def _find_objects_with_choreographed_moves( return all_detections, all_artifacts # Fail. Analyze the RGBDs if you want (by uncommenting here). - # import imageio.v2 as iio - # for i, rgbds in enumerate(all_rgbds): - # for camera, rgbd in rgbds.items(): - # path = f"init_search_for_objects_angle{i}_{camera}.png" - # iio.imsave(path, rgbd.rgb) - # print(f"Wrote out to {path}.") + import imageio.v2 as iio + for i, rgbds in enumerate(all_rgbds): + for camera, rgbd in rgbds.items(): + path = f"init_search_for_objects_angle{i}_{camera}.png" + iio.imsave(path, rgbd.rgb) + print(f"Wrote out to {path}.") remaining_object_ids = set(object_ids) - set(all_detections) raise RuntimeError(f"Could not find objects: {remaining_object_ids}") diff --git a/predicators/spot_utils/skills/spot_wipe_table.py b/predicators/spot_utils/skills/spot_wipe_table.py new file mode 100644 index 0000000000..b2be17a41e --- /dev/null +++ b/predicators/spot_utils/skills/spot_wipe_table.py @@ -0,0 +1,154 @@ +"""Interface for spot sweeping skill.""" + +import time +from typing import Tuple + +import numpy as np +from bosdyn.client import math_helpers +from bosdyn.client.sdk import Robot + +from predicators.spot_utils.skills.spot_hand_move import \ + move_hand_to_relative_pose, move_hand_to_relative_pose_with_velocity + + +def wipe_one_stroke(robot: Robot, wipe_start_pose: math_helpers.SE3Pose, + move_dx: float, move_dy: float, duration: float) -> None: + """Wipe a table surface in the xy plane. + + The robot starts at a start pose, and then moves forward and back by + dx and dy. + """ + # Move first in the yz direction (perpendicular to robot body) to avoid + # knocking the target object over. + move_hand_to_relative_pose(robot, wipe_start_pose) + first_move_pose = math_helpers.SE3Pose( + x=wipe_start_pose.x + move_dx, # sensible default + y=wipe_start_pose.y + move_dy, + z=wipe_start_pose.z, + rot=wipe_start_pose.rot, + ) + move_hand_to_relative_pose_with_velocity(robot, wipe_start_pose, + first_move_pose, duration) + time.sleep(0.1) + # Move back to the start pose. + move_hand_to_relative_pose_with_velocity(robot, first_move_pose, + wipe_start_pose, duration) + + +def wipe_multiple_strokes(robot: Robot, wipe_start_pose: math_helpers.SE3Pose, + end_look_pose: math_helpers.SE3Pose, + stroke_dx: float, stroke_dy: float, + delta_x_y_between_strokes: Tuple[float, float], + num_strokes: int, + duration_per_stroke: float) -> None: + """Wipe a table surface in the xy plane. + + The robot starts at a start pose, and then moves forward and back by + dx and dy. + """ + curr_stroke_start_pose = wipe_start_pose + for i in range(num_strokes): + move_hand_to_relative_pose(robot, curr_stroke_start_pose) + first_move_pose = math_helpers.SE3Pose( + x=curr_stroke_start_pose.x + stroke_dx, + y=curr_stroke_start_pose.y + stroke_dy, + z=curr_stroke_start_pose.z, + rot=curr_stroke_start_pose.rot, + ) + move_hand_to_relative_pose_with_velocity(robot, curr_stroke_start_pose, + first_move_pose, + duration_per_stroke) + time.sleep(0.1) + # Move back to the start pose. + move_hand_to_relative_pose_with_velocity(robot, first_move_pose, + curr_stroke_start_pose, + duration_per_stroke) + # Move to the next stroke position. + curr_stroke_start_pose = math_helpers.SE3Pose( + x=curr_stroke_start_pose.x + delta_x_y_between_strokes[0], + y=curr_stroke_start_pose.y + delta_x_y_between_strokes[1], + z=curr_stroke_start_pose.z, + rot=curr_stroke_start_pose.rot, + ) + # Move to the end look pose. + move_hand_to_relative_pose(robot, end_look_pose) + + +if __name__ == "__main__": + # Run this file alone to test manually. + # Make sure to pass in --spot_robot_ip. + + # NOTE: this test assumes that the robot is standing in front of a table + # that has a train_toy on it. The test starts by running object detection to + # get the pose of the train_toy. Then the robot opens its gripper and pauses + # until a brush is put in the gripper, with the bristles facing down and + # forward. The robot should then brush the train_toy to the right. + + # pylint: disable=ungrouped-imports + from bosdyn.client import create_standard_sdk + from bosdyn.client.lease import LeaseClient, LeaseKeepAlive + from bosdyn.client.util import authenticate + + from predicators import utils + from predicators.settings import CFG + from predicators.spot_utils.perception.perception_structs import \ + LanguageObjectDetectionID + from predicators.spot_utils.skills.spot_find_objects import \ + init_search_for_objects + from predicators.spot_utils.skills.spot_hand_move import close_gripper, \ + open_gripper + from predicators.spot_utils.skills.spot_navigation import go_home + from predicators.spot_utils.spot_localization import SpotLocalizer + from predicators.spot_utils.utils import get_graph_nav_dir, \ + get_relative_se2_from_se3, verify_estop + + def _run_manual_test() -> None: + # Put inside a function to avoid variable scoping issues. + args = utils.parse_args(env_required=False, + seed_required=False, + approach_required=False) + utils.update_config(args) + + # Get constants. + hostname = CFG.spot_robot_ip + + sdk = create_standard_sdk('WipeSkillTestClient') + robot = sdk.create_robot(hostname) + authenticate(robot) + verify_estop(robot) + lease_client = robot.ensure_client(LeaseClient.default_service_name) + lease_client.take() + robot.time_sync.wait_for_sync() + + # Move the hand to the side. + hand_side_pose = math_helpers.SE3Pose(x=0.80, + y=0.0, + z=0.25, + rot=math_helpers.Quat.from_yaw( + -np.pi / 2)) + move_hand_to_relative_pose(robot, hand_side_pose) + # Ask for the eraser. + open_gripper(robot) + # Press any key, instead of just enter. Useful for remote control. + msg = "Put the brush in the robot's gripper, then press any key" + utils.wait_for_any_button_press(msg) + close_gripper(robot) + + # NOTE: these parameters hardcoded for a particular child_play_table + # object njk is experimenting with. Please swap out depending on the + # actual object you have + start_pose = math_helpers.SE3Pose(x=0.85, + y=-0.2, + z=-0.08, + rot=math_helpers.Quat.from_pitch( + np.pi / 2)) + end_pose = math_helpers.SE3Pose(x=0.65, + y=0.0, + z=0.4, + rot=math_helpers.Quat.from_pitch( + np.pi / 2.5)) + # Execute the sweep. + wipe_multiple_strokes(robot, start_pose, end_pose, 0.0, 0.4, + (0.05, 0.0), 5, 1.0) + + _run_manual_test() diff --git a/predicators/spot_utils/spot_record_option_demo.py b/predicators/spot_utils/spot_record_option_demo.py new file mode 100644 index 0000000000..67e896c83f --- /dev/null +++ b/predicators/spot_utils/spot_record_option_demo.py @@ -0,0 +1,55 @@ +"""Script to make it easy to record demonstrations from the Spot.""" + +from pathlib import Path + +from predicators import utils +from predicators.perception.spot_perceiver import \ + annotate_imgs_with_detections, save_annotated_imgs_for_vlm_demo +from predicators.settings import CFG +from predicators.spot_utils.perception.spot_cameras import \ + capture_images_without_context +from predicators.spot_utils.utils import verify_estop + + +def main(): + # Run this file alone to test manually. + # Make sure to pass in --spot_robot_ip. + + # pylint: disable=ungrouped-imports + import numpy as np + from bosdyn.client import create_standard_sdk + from bosdyn.client.lease import LeaseClient + from bosdyn.client.util import authenticate + + # Put inside a function to avoid variable scoping issues. + args = utils.parse_args(env_required=False, + seed_required=False, + approach_required=False) + utils.update_config(args) + + # Get constants. + hostname = CFG.spot_robot_ip + + # Instantiate a robot. + sdk = create_standard_sdk('MoveHandSkillTestClient') + robot = sdk.create_robot(hostname) + authenticate(robot) + verify_estop(robot) + lease_client = robot.ensure_client(LeaseClient.default_service_name) + lease_client.take() + + assert len(CFG.spot_vlm_teleop_demo_folderpath + ) > 0, "Please set the spot_vlm_teleop_demo_folderpath!" + + # Pull all the images from the spot cameras and annotate them + # with the camera names. + # NOTE: we currently don't run any object detection, though we could. + rgbd_images = capture_images_without_context(robot) + annotated_imgs = annotate_imgs_with_detections(rgbd_images, {}) + # Save the images properly. + save_annotated_imgs_for_vlm_demo(annotated_imgs, + Path(CFG.spot_vlm_teleop_demo_folderpath)) + + +if __name__ == '__main__': + main() diff --git a/predicators/spot_utils/utils.py b/predicators/spot_utils/utils.py index d9e046a013..69475c366a 100644 --- a/predicators/spot_utils/utils.py +++ b/predicators/spot_utils/utils.py @@ -87,6 +87,9 @@ class _Spot3DShape(Enum): _container_type = Type("container", list(_movable_object_type.feature_names), parent=_movable_object_type) +_trash_can_type = Type("trashcan", + list(_immovable_object_type.feature_names), + parent=_immovable_object_type) _dustpan_type = Type("dustpan", list(_movable_object_type.feature_names), parent=_movable_object_type) @@ -96,6 +99,9 @@ class _Spot3DShape(Enum): _wrappers_type = Type("wrappers", list(_movable_object_type.feature_names), parent=_movable_object_type) +_table_type = Type("table", + list(_immovable_object_type.feature_names), + parent=_immovable_object_type) def get_collision_geoms_for_nav(state: State) -> List[_Geom2D]: @@ -366,7 +372,11 @@ def sample_move_offset_from_target( """ for _ in range(max_samples): distance = rng.uniform(min_distance, max_distance) - angle = rng.uniform(min_angle, max_angle) + try: + angle = rng.uniform(min_angle, max_angle) + except ValueError: + import ipdb + ipdb.set_trace() dx = np.cos(angle) * distance dy = np.sin(angle) * distance x = target_origin[0] + dx diff --git a/predicators/utils.py b/predicators/utils.py index 2e7c59c383..e0abf8a01f 100644 --- a/predicators/utils.py +++ b/predicators/utils.py @@ -2551,13 +2551,12 @@ def get_prompt_for_vlm_state_labelling( if "img_option_diffs" in prompt_type: # In this case, we need to load the 'per_scene_naive' prompt as well # for the first timestep. - with open(filepath_prefix + "per_scene_naive.txt", - "r", + with open(filepath_prefix + "per_scene_cot.txt", "r", encoding="utf-8") as f: init_prompt = f.read() for atom_str in atoms_list: init_prompt += f"\n{atom_str}" - if len(label_history) == 0: + if len(label_history) == 0 or (skill_history[-1] is None): return (init_prompt, imgs_history[0]) # Now, we use actual difference-based prompting for the second timestep # and beyond. @@ -2658,33 +2657,71 @@ def query_vlm_for_atom_vals( assert len(vlm_output) == 1 vlm_output_str = vlm_output[0] logging.info(f"VLM output: \n{vlm_output_str}") - # Parse out stuff. - if len(label_history) > 0: # pragma: no cover - truth_values = re.findall(r'\* (.*): (True|False)', vlm_output_str) - for i, (atom_query, - pred_label) in enumerate(zip(atom_queries_list, truth_values)): - pred, label = pred_label - assert pred in atom_query - label = label.lower() - if label == "true": - true_atoms.add(vlm_atoms[i]) - else: - all_vlm_responses = vlm_output_str.strip().split("\n") - # NOTE: this assumption is likely too brittle; if this is breaking, - # feel free to remove/adjust this and change the below parsing - # loop accordingly! - if len(atom_queries_list) != len(all_vlm_responses): - return true_atoms - for i, (atom_query, curr_vlm_output_line) in enumerate( - zip(atom_queries_list, all_vlm_responses)): - assert atom_query + ":" in curr_vlm_output_line - assert "." in curr_vlm_output_line - # period_idx = curr_vlm_output_line.find(".") - # value = curr_vlm_output_line[len(atom_query + ":"): - # period_idx].lower().strip() - value = curr_vlm_output_line.split(': ')[-1].strip('.').lower() - if value == "true": - true_atoms.add(vlm_atoms[i]) + # Parse the VLM output to find true atoms. + true_atoms = set() + # Create a mapping from the query string back to the GroundAtom object. + query_str_to_atom = {atom.get_vlm_query_str(): atom for atom in vlm_atoms} + for atom_query_str in atom_queries_list: + # Escape special characters in the atom query string for regex. + escaped_query = re.escape(atom_query_str) + # Regex to find the atom query string followed by a colon (optional) + # and a truth value (True, False, Unknown), case-insensitive. + # We look for the value potentially surrounded by whitespace. + pattern = re.compile(rf"{escaped_query}\s*:?\s*(True|False|Unknown)", + re.IGNORECASE) + matches = list(pattern.finditer(vlm_output_str)) + if matches: + # Find the last match. + last_match = matches[-1] + # Extract the truth value from the last match. + truth_value_str = last_match.group(1) + # Check if the truth value is 'True' (case-insensitive). + if truth_value_str.lower() == 'true': + # Find the corresponding GroundAtom object. + if atom_query_str in query_str_to_atom: + true_atoms.add(query_str_to_atom[atom_query_str]) + else: + # This case should ideally not happen if atom_queries_list + # is derived correctly from vlm_atoms. + logging.warning( + f"Could not find GroundAtom for query: {atom_query_str}" + ) + logging.info(f"Parsed true atoms: {true_atoms}") + # # Parse out stuff. + # if len(label_history) > 0: # pragma: no cover + # truth_values = re.findall(r'\* (.*): (True|False|Unknown)', + # vlm_output_str) + # for i, (atom_query, + # pred_label) in enumerate(zip(atom_queries_list, truth_values)): + # pred, label = pred_label + # pred = pred.strip() + # try: + # assert pred in atom_query + # except AssertionError: + # import ipdb + # ipdb.set_trace() + # label = label.lower() + # if "true" in label.lower(): + # true_atoms.add(vlm_atoms[i]) + # else: + # if "Predicate Values:" in vlm_output_str: + # all_vlm_responses = vlm_output_str.strip().split("\n") + # # NOTE: this assumption is likely too brittle; if this is breaking, + # # feel free to remove/adjust this and change the below parsing + # # loop accordingly! + # if len(atom_queries_list) != len(all_vlm_responses): + # import ipdb; ipdb.set_trace() + # return true_atoms + # for i, (atom_query, curr_vlm_output_line) in enumerate( + # zip(atom_queries_list, all_vlm_responses)): + # assert atom_query + ":" in curr_vlm_output_line + # assert "." in curr_vlm_output_line + # # period_idx = curr_vlm_output_line.find(".") + # # value = curr_vlm_output_line[len(atom_query + ":"): + # # period_idx].lower().strip() + # value = curr_vlm_output_line.split(': ')[-1].strip('.').lower() + # if "true" in value: + # true_atoms.add(vlm_atoms[i]) return true_atoms @@ -2699,7 +2736,11 @@ def abstract(state: State, try: if state.simulator_state is not None and "abstract_state" in \ state.simulator_state: # pragma: no cover - return state.simulator_state["abstract_state"] + return { + atom + for atom in state.simulator_state["abstract_state"] + if atom.predicate in preds + } except (AttributeError, TypeError): pass # Start by pulling out all VLM predicates. diff --git a/run_predicates.py b/run_predicates.py new file mode 100644 index 0000000000..089bb91bfb --- /dev/null +++ b/run_predicates.py @@ -0,0 +1,74 @@ +import numpy as np +import json +from predicators.envs.spot_env import LISSpotCollectEnv +from predicators.perception.spot_perceiver import SpotPerceiver +from predicators.ground_truth_models import get_gt_nsrts, get_gt_options +from predicators.structs import Object, State, GroundAtom +from predicators.spot_utils.utils import _container_type, _movable_object_type, _robot_type +from predicators import utils + +utils.reset_config({ + "env": + "lis_spot_collect_misplaced_items_env", + "approach": + "spot_wrapper[oracle]", + "num_train_tasks": + 0, + "num_test_tasks": + 1, + "seed": + 0, + "spot_run_dry": + True, + "spot_robot_ip": + None, + "spot_graph_nav_map": + "b45-621", + "bilevel_plan_without_sim": + True, + "perceiver": + "spot_perceiver", + "spot_use_perfect_samplers": + True, +}) + +rng = np.random.default_rng(123) +env = LISSpotCollectEnv() +perceiver = SpotPerceiver() +nsrts = get_gt_nsrts(env.get_name(), env.predicates, + get_gt_options(env.get_name())) + +pred_name_to_pred = {p.name: p for p in env.predicates} + +robot = Object("robot", _robot_type) +handle = Object("green_handle", _movable_object_type) +blue_block = Object("blue_block", _movable_object_type) +yellow_cup = Object("yellow_cup", _movable_object_type) +toy_plane = Object("toy_plane", _movable_object_type) +cardboard_box = Object("cardboard_box", _container_type) +Inside = pred_name_to_pred["Inside"] + +# state from json +state = State({}, simulator_state=None) + +# get all grounded atoms +all_grounded_atoms = None + +json_file = "/Users/shashlik/Desktop/last.json" +with open(json_file, "r", encoding="utf-8") as f: + json_dict = json.load(f) +object_name_to_object = env._parse_object_name_to_object_from_json( + json_dict) +init_dict = env._parse_init_state_dict_from_json( + json_dict, object_name_to_object) +for i, (obj, init_val) in enumerate(sorted(init_dict.items())): + init_val["object_id"] = i +state = utils.create_state_from_dict(init_dict) + +a = GroundAtom(Inside, [blue_block, cardboard_box]) +print(a, a.holds(state)) + +import ipdb; ipdb.set_trace() + +for a in all_grounded_atoms: + a.holds(state) \ No newline at end of file