From 16fa9014edbe767ddef3e72d8c8652b38d0f02e8 Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Mon, 18 Dec 2023 17:55:22 -0500 Subject: [PATCH 01/11] problem_generator.py: Simplified config, and runs with default args again --- .../pddl/jem_survey_template.pddl | 10 +- .../pddl/problem_jem_survey.pddl | 135 +++++++++--------- .../survey_planner/tools/problem_generator.py | 84 ++++++----- 3 files changed, 119 insertions(+), 110 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl index 313ca28f..c9173ff5 100644 --- a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl @@ -48,10 +48,6 @@ ;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. -;; BEGIN CONFIG_DYNAMIC -{{ config_dynamic }} -;; END CONFIG_DYNAMIC - -;; BEGIN CONFIG_STATIC -{{ config_static }} -;; END CONFIG_STATIC +;; BEGIN CONFIG +{{ config }} +;; END CONFIG diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl index 301f8c05..f1855d02 100644 --- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -1,7 +1,9 @@ ;; Auto-generated by problem_generator.py. Do not edit! -;; Problem template: jem_survey_template.pddl -;; Dynamic config: jem_survey_dynamic.yaml -;; Static config: jem_survey_static.yaml +;; Command was: ./tools/problem_generator.py +;; Working directory was: /home/vagrant/isaac/astrobee/survey_manager/survey_planner +;; Problem template: pddl/jem_survey_template.pddl +;; Config 1: data/jem_survey_static.yaml +;; Config 2: data/jem_survey_dynamic.yaml (define (problem jem-survey) (:domain survey-manager) @@ -174,7 +176,7 @@ ;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. -;; BEGIN CONFIG_DYNAMIC +;; BEGIN CONFIG ;; # Copyright (c) 2023, United States Government, as represented by the ;; # Administrator of the National Aeronautics and Space Administration. ;; # @@ -192,65 +194,7 @@ ;; # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the ;; # License for the specific language governing permissions and limitations ;; # under the License. -;; -;; # Example dynamic configuration info used when generating a PDDL problem. For now, this is goal -;; # conditions and initial state. A likely conops is that the initial version of this file for a -;; # specific activity would be hand-generated, but it might later be automatically regenerated by the -;; # survey manager when a replan is needed (remove completed/failed goals, add retry goals, update -;; # initial state to match actual current state, etc.) See also jem_survey_static.yaml. -;; -;; goals: -;; -;; - {type: panorama, robot: bumble, order: 0, location: jem_bay4, run: 1} -;; - {type: panorama, robot: bumble, order: 1, location: jem_bay3, run: 1} -;; - {type: panorama, robot: bumble, order: 2, location: jem_bay2, run: 1} -;; - {type: panorama, robot: bumble, order: 3, location: jem_bay1, run: 1} -;; - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3, run: 1} -;; -;; # We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to -;; # get confused and greatly increase the total run time. For some reason, it doesn't notice it can -;; # use the same plan as without this goal and then add some motion actions at the end to achieve this -;; # goal. Instead, it falls back to only undocking one robot at a time, which slows things down by -;; # about 2x. -;; # - {type: robot_at, robot: bumble, location: berth1} -;; -;; - {type: panorama, robot: honey, order: 0, location: jem_bay7, run: 1} -;; - {type: panorama, robot: honey, order: 1, location: jem_bay6, run: 1} -;; - {type: panorama, robot: honey, order: 2, location: jem_bay5, run: 1} -;; -;; # This is another objective we want to include that for some reason causes POPF to fail to generate -;; # a plan (hang indefinitely). No obvious reason why it should cause a problem. -;; # - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7, run: 1} -;; -;; - {type: robot_at, robot: honey, location: berth2} -;; -;; init: -;; bumble: -;; location: berth1 -;; honey: -;; location: berth2 -;; -;; END CONFIG_DYNAMIC - -;; BEGIN CONFIG_STATIC -;; # Copyright (c) 2023, United States Government, as represented by the -;; # Administrator of the National Aeronautics and Space Administration. -;; # -;; # All rights reserved. -;; # -;; # The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking -;; # platform" software is licensed under the Apache License, Version 2.0 -;; # (the "License"); you may not use this file except in compliance with the -;; # License. You may obtain a copy of the License at -;; # -;; # http://www.apache.org/licenses/LICENSE-2.0 -;; # -;; # Unless required by applicable law or agreed to in writing, software -;; # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -;; # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -;; # License for the specific language governing permissions and limitations -;; # under the License. -;; +;; ;; # Static configuration info used when generating a PDDL problem and also when executing actions in a ;; # PDDL plan. This info should be static in the sense that it nominally doesn't change during an ISS ;; # activity, so the survey manager doesn't have to modify it. However, an edge case is that an @@ -258,10 +202,10 @@ ;; # the position of a named bay away from an obstacle) and restart the survey manager. On the other ;; # hand, info that is *expected* to change as part of the survey manager conops belongs in ;; # jem_survey_dynamic.yaml. -;; +;; ;; # Useful reference for positions and stereo survey trajectories: ;; # https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure -;; +;; ;; bays: ;; # 3D coordinates for symbolic bays in ISS Analysis Coordinate System used by Astrobee ;; jem_bay1: [11.0, -4.0, 4.8] @@ -271,12 +215,12 @@ ;; jem_bay5: [11.0, -8.0, 4.8] ;; jem_bay6: [11.0, -9.0, 4.8] ;; jem_bay7: [11.0, -9.7, 4.8] -;; +;; ;; bogus_bays: [jem_bay0, jem_bay8] ;; berths: [berth1, berth2] ;; robots: [bumble, honey] ;; num_orders: 10 -;; +;; ;; stereo: ;; # Meta-data about stereo survey options ;; jem_bay1_to_bay3: @@ -292,5 +236,58 @@ ;; fplan: "jem_stereo_mapping_bay4_to_bay7.fplan" ;; base_location: jem_bay7 ;; bound_location: jem_bay4 -;; -;; END CONFIG_STATIC +;; # Copyright (c) 2023, United States Government, as represented by the +;; # Administrator of the National Aeronautics and Space Administration. +;; # +;; # All rights reserved. +;; # +;; # The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +;; # platform" software is licensed under the Apache License, Version 2.0 +;; # (the "License"); you may not use this file except in compliance with the +;; # License. You may obtain a copy of the License at +;; # +;; # http://www.apache.org/licenses/LICENSE-2.0 +;; # +;; # Unless required by applicable law or agreed to in writing, software +;; # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +;; # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +;; # License for the specific language governing permissions and limitations +;; # under the License. +;; +;; # Example dynamic configuration info used when generating a PDDL problem. For now, this is goal +;; # conditions and initial state. A likely conops is that the initial version of this file for a +;; # specific activity would be hand-generated, but it might later be automatically regenerated by the +;; # survey manager when a replan is needed (remove completed/failed goals, add retry goals, update +;; # initial state to match actual current state, etc.) See also jem_survey_static.yaml. +;; +;; goals: +;; +;; - {type: panorama, robot: bumble, order: 0, location: jem_bay4, run: 1} +;; - {type: panorama, robot: bumble, order: 1, location: jem_bay3, run: 1} +;; - {type: panorama, robot: bumble, order: 2, location: jem_bay2, run: 1} +;; - {type: panorama, robot: bumble, order: 3, location: jem_bay1, run: 1} +;; - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3, run: 1} +;; +;; # We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to +;; # get confused and greatly increase the total run time. For some reason, it doesn't notice it can +;; # use the same plan as without this goal and then add some motion actions at the end to achieve this +;; # goal. Instead, it falls back to only undocking one robot at a time, which slows things down by +;; # about 2x. +;; # - {type: robot_at, robot: bumble, location: berth1} +;; +;; - {type: panorama, robot: honey, order: 0, location: jem_bay7, run: 1} +;; - {type: panorama, robot: honey, order: 1, location: jem_bay6, run: 1} +;; - {type: panorama, robot: honey, order: 2, location: jem_bay5, run: 1} +;; +;; # This is another objective we want to include that for some reason causes POPF to fail to generate +;; # a plan (hang indefinitely). No obvious reason why it should cause a problem. +;; # - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7, run: 1} +;; +;; - {type: robot_at, robot: honey, location: berth2} +;; +;; init: +;; bumble: +;; location: berth1 +;; honey: +;; location: berth2 +;; END CONFIG diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py index 7b5b2b31..035107e2 100755 --- a/astrobee/survey_manager/survey_planner/tools/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -32,13 +32,22 @@ import itertools import pathlib import re +import shlex import sys -from typing import Any, Dict, Iterable, Sequence, T, Tuple +from typing import Any, Dict, Iterable, List, Sequence, T, Tuple import yaml GOAL_TYPE_OPTIONS = ("panorama", "stereo", "robot_at") +THIS_DIR = pathlib.Path(__file__).resolve().parent +CWD = pathlib.Path.cwd() +DATA_DIR = (THIS_DIR / ".." / "data").resolve().relative_to(CWD) +PDDL_DIR = (THIS_DIR / ".." / "pddl").resolve().relative_to(CWD) +DEFAULT_CONFIGS = [ + DATA_DIR / "jem_survey_static.yaml", + DATA_DIR / "jem_survey_dynamic.yaml", +] # Type alias YamlMapping = Dict[str, Any] @@ -140,7 +149,7 @@ def __call__(self, match: re.match) -> str: param = match.group(1) if param not in self.params: raise KeyError( - "Expected template param in {self.params.keys()}, got {{{{ {param} }}}}" + f"Template param {{{{ {param} }}}} not found in {list(self.params.keys())}" ) return self.params[param] @@ -149,46 +158,51 @@ def comment_for_pddl(text: str) -> str: """ Return the result of commenting `text` using PDDL (Lisp-like) comment syntax. """ - return ";; " + text.replace("\n", "\n;; ") + return "\n".join([f";; {line}".strip() for line in text.splitlines()]) def problem_generator( problem_template_path: pathlib.Path, - config_dynamic_path: pathlib.Path, - config_static_path: pathlib.Path, + config_paths: Iterable[pathlib.Path], output_path: pathlib.Path, + command: str, ) -> None: """ The main function that generates the problem. """ problem_template = problem_template_path.read_text() - config_dynamic = load_yaml(config_dynamic_path) - config_static = load_yaml(config_static_path) + config = {} + for config_path in config_paths: + config.update(load_yaml(config_path)) params = {} + yaml.safe_dump(config, stream=sys.stdout) - params["header"] = ( + header_lines = ( ";; Auto-generated by problem_generator.py. Do not edit!\n" + f";; Command was: {command}\n" + f";; Working directory was: {CWD}\n" f";; Problem template: {problem_template_path}\n" - f";; Dynamic config: {config_dynamic_path}\n" - f";; Static config: {config_static_path}\n" ) + for i, config_path in enumerate(config_paths): + header_lines += f";; Config {i + 1}: {config_path}\n" + params["header"] = header_lines - bays = list(config_static["bays"].keys()) - bogus_bays = config_static["bogus_bays"] + bays = list(config["bays"].keys()) + bogus_bays = config["bogus_bays"] all_bays = sorted(bays + bogus_bays) params["bays"] = " ".join(all_bays) - berths = config_static["berths"] + berths = config["berths"] params["berths"] = " ".join(berths) - robots = config_static["robots"] + robots = config["robots"] params["robots"] = " ".join(robots) - num_orders = config_static["num_orders"] + num_orders = config["num_orders"] params["orders"] = " ".join([f"o{i}" for i in range(num_orders)]) - yaml_goals = config_dynamic["goals"] - pddl_goals = [pddl_goal_from_yaml(goal, config_static) for goal in yaml_goals] + yaml_goals = config["goals"] + pddl_goals = [pddl_goal_from_yaml(goal, config) for goal in yaml_goals] params["goals"] = indent_lines(pddl_goals, 12) move_connected_lines = [ @@ -222,7 +236,7 @@ def problem_generator( robot_available_lines = [f"(robot-available {robot})" for robot in robots] params["robot_available_predicates"] = indent_lines(robot_available_lines, 8) - init = config_dynamic["init"] + init = config["init"] robot_at_lines = [ f"(robot-at {robot} {init[robot]['location']})" for robot in robots ] @@ -249,8 +263,10 @@ def problem_generator( robot_order_lines = [f"(= (robot-order {robot}) -1)" for robot in robots] params["robot_order_fluents"] = indent_lines(robot_order_lines, 8) - params["config_dynamic"] = comment_for_pddl(config_dynamic_path.read_text()) - params["config_static"] = comment_for_pddl(config_static_path.read_text()) + config_text = "" + for config_path in config_paths: + config_text += config_path.read_text() + params["config"] = comment_for_pddl(config_text) # print(yaml.safe_dump(params, indent=4, sort_keys=False)) filled_template = TEMPLATE_SUBST_REGEX.sub(TemplateFiller(params), problem_template) @@ -264,7 +280,13 @@ class CustomFormatter( pass +def path_list(paths_text: str) -> List[pathlib.Path]: + cwd = pathlib.Path.cwd() + return [pathlib.Path(pstr) for pstr in paths_text.split(",")] + + def main(): + cwd = pathlib.Path.cwd() parser = argparse.ArgumentParser( description=__doc__, formatter_class=CustomFormatter ) @@ -272,34 +294,28 @@ def main(): "--problem-template", help="Path to input PDDL problem template", type=pathlib.Path, - default="jem_survey_template.pddl", - ) - parser.add_argument( - "--config-dynamic", - help="Path to input dynamic problem config YAML (goals, initial state)", - type=pathlib.Path, - default="jem_survey_dynamic.yaml", + default=PDDL_DIR / "jem_survey_template.pddl", ) parser.add_argument( - "--config-static", - help="Path to input static problem config YAML (module geometry, available stereo surveys, etc.)", - type=pathlib.Path, - default="jem_survey_static.yaml", + "--config", + help="Comma-separated list of paths to YAML problem config inputs", + type=path_list, + default=",".join([str(path) for path in DEFAULT_CONFIGS]) ) parser.add_argument( "-o", "--output", help="Path for output PDDL problem", type=pathlib.Path, - default="problem_jem_survey.pddl", + default=PDDL_DIR / "problem_jem_survey.pddl", ) args = parser.parse_args() problem_generator( problem_template_path=args.problem_template, - config_dynamic_path=args.config_dynamic, - config_static_path=args.config_static, + config_paths=args.config, output_path=args.output, + command=shlex.join(sys.argv), ) From 76223a00e6f6a95e32beeada1cec3814e7ab7b8c Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Mon, 18 Dec 2023 18:16:57 -0500 Subject: [PATCH 02/11] Removed run number argument from planner actions --- .../data/sample_output_plan.txt | 46 +++++++++---------- .../survey_planner/pddl/domain_survey.pddl | 33 ++++++------- .../pddl/jem_survey_template.pddl | 1 - .../pddl/problem_jem_survey.pddl | 19 ++++---- .../survey_planner/tools/plan_interpreter.py | 9 ++-- .../survey_planner/tools/problem_generator.py | 12 ++--- 6 files changed, 51 insertions(+), 69 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt index 6a119031..03eb0bd5 100644 --- a/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt @@ -1,31 +1,31 @@ -$ time popf domain_survey.pddl problem_jem_survey.pddl +$ time popf pddl/domain_survey.pddl pddl/problem_jem_survey.pddl Constructing lookup tables: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] Post filtering unreachable actions: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] -3% of the ground temporal actions in this problem are compression-safe -b (25.000 | 30.000)b (24.000 | 50.001)b (23.000 | 70.002)b (22.000 | 90.003)b (21.000 | 990.004)b (20.000 | 990.004)b (19.000 | 1010.005)b (18.000 | 1910.006)b (17.000 | 1910.006)b (16.000 | 1930.007)b (15.000 | 2830.008)b (14.000 | 2830.008)b (13.000 | 2850.009)b (12.000 | 3750.010)b (11.000 | 3750.010)b (10.000 | 4350.011)b (9.000 | 4350.011)b (8.000 | 4350.011)b (7.000 | 4350.011)b (6.000 | 4350.011)b (5.000 | 4350.011)b (4.000 | 5270.013)b (3.000 | 5270.013)b (2.000 | 5290.014)b (1.000 | 5310.015);;;; Solution Found -; Time 0.56 +16% of the ground temporal actions in this problem are compression-safe +b (25.000 | 30.000)b (24.000 | 50.001)b (23.000 | 70.002)b (22.000 | 90.003)b (21.000 | 870.004)b (20.000 | 870.004)b (19.000 | 890.005)b (18.000 | 1670.006)b (17.000 | 1670.006)b (16.000 | 1690.007)b (15.000 | 2470.008)b (14.000 | 2470.008)b (13.000 | 2490.009)b (12.000 | 3270.010)b (11.000 | 3270.010)b (10.000 | 3870.011)b (9.000 | 3870.011)b (8.000 | 3870.011)b (7.000 | 3870.011)b (6.000 | 3870.011)b (5.000 | 3870.011)b (4.000 | 4670.013)b (3.000 | 4670.013)b (2.000 | 4690.014)b (1.000 | 4710.015);;;; Solution Found +; Time 0.21 0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) [30.000] 30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] 50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] 70.003: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] 70.003: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] -90.004: (panorama bumble o0 jem_bay4 run1) [900.000] -100.004: (panorama honey o0 jem_bay7 run1) [900.000] -990.005: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] -1000.005: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] -1010.006: (panorama bumble o1 jem_bay3 run1) [900.000] -1020.006: (panorama honey o1 jem_bay6 run1) [900.000] -1910.007: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] -1930.008: (panorama bumble o2 jem_bay2 run1) [900.000] -2830.009: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] -2850.010: (panorama bumble o3 jem_bay1 run1) [900.000] -3750.011: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3 run1) [600.000] -4350.012: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] -4370.013: (panorama honey o2 jem_bay5 run1) [900.000] -5270.014: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] -5290.015: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] -5310.016: (dock honey jem_bay7 berth2) [30.000] +90.004: (panorama bumble o0 jem_bay4) [780.000] +100.004: (panorama honey o0 jem_bay7) [780.000] +870.005: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +880.005: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +890.006: (panorama bumble o1 jem_bay3) [780.000] +900.006: (panorama honey o1 jem_bay6) [780.000] +1670.007: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +1690.008: (panorama bumble o2 jem_bay2) [780.000] +2470.009: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] +2490.010: (panorama bumble o3 jem_bay1) [780.000] +3270.011: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) [600.000] +3870.012: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] +3890.013: (panorama honey o2 jem_bay5) [780.000] +4670.014: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] +4690.015: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +4710.016: (dock honey jem_bay7 berth2) [30.000] -real 0m0.585s -user 0m0.553s -sys 0m0.032s +real 0m0.221s +user 0m0.208s +sys 0m0.013s diff --git a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl index e31999fd..36041f47 100644 --- a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl @@ -10,7 +10,6 @@ location robot order - run-number ) (:predicates @@ -67,35 +66,31 @@ ;; preconditions of the stereo action, and one of its effects is to clear the ;; predicate. Therefore, the planner won't waste time trying to execute stereo actions that ;; the user didn't explicitly request. Without this hack, the planner run time blows up. - (need-stereo ?robot - robot ?order - order ?base ?bound - location ?run-number - run-number) + (need-stereo ?robot - robot ?order - order ?base ?bound - location) ;; === Goal predicates === ;; completed-panorama: The goal to add if you want the plan to include collecting a ;; panorama. For now, goals specify ?robot and ?order parameters that constrain - ;; multi-robot task allocation and task ordering. The ?run-number is used to indicate - ;; retries and is meaningless to the planner but helpful for post-run analysis. + ;; multi-robot task allocation and task ordering. (completed-panorama ?robot - robot ?order - order ?location - location - ?run-number - run-number ) ;; completed-stereo: The goal to add if you want the plan to include collecting a stereo ;; survey. For now, goals specify ?robot and ?order parameters that constrain multi-robot - ;; task allocation and task ordering. The ?run-number is used to indicate retries and is - ;; meaningless to the planner but helpful for post-run analysis. The current model for - ;; stereo surveys assumes the robot starts and ends the survey at the same location called - ;; ?base (these locations only need to be same to the effective precision modeled in the - ;; planner, "less than a bay apart"). The ?bound argument indicates the other end of the - ;; interval covered by the survey and is used for collision checking. It's assumed that - ;; ?base and ?bound are not adjacent locations. If future stereo surveys violate these - ;; assumptions the model will need to be revisited. + ;; task allocation and task ordering. The current model for stereo surveys assumes the robot + ;; starts and ends the survey at the same location called ?base (these locations only need + ;; to be same to the effective precision modeled in the planner, "less than a bay apart"). + ;; The ?bound argument indicates the other end of the interval covered by the survey and is + ;; used for collision checking. It's assumed that ?base and ?bound are not adjacent + ;; locations. If future stereo surveys violate these assumptions the model will need to be + ;; revisited. (completed-stereo ?robot - robot ?order - order ?base ?bound - location - ?run-number - run-number ) ) @@ -237,7 +232,6 @@ ?robot - robot ?order - order ?location - location - ?run-number - run-number ) ;; ~13 minutes, per https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure :duration (= ?duration 780) @@ -262,7 +256,7 @@ (at end (assign (robot-order ?robot) (order-identity ?order))) ;; Mark success - (at end (completed-panorama ?robot ?order ?location ?run-number)) + (at end (completed-panorama ?robot ?order ?location)) ) ) @@ -275,7 +269,6 @@ ?base ?bound - location ;; ?check1 and ?check2: Planner-selected neighbors of ?bound for collision check ?check1 ?check2 - location - ?run-number - run-number ) :duration (= ?duration 600) ;; 10 minutes :condition @@ -292,7 +285,7 @@ ;; Check for need-stereo so the planner only tries this action when the user ;; explicitly requests it. - (at start (need-stereo ?robot ?order ?base ?bound ?run-number)) + (at start (need-stereo ?robot ?order ?base ?bound)) ;; Check collision avoidance (at start (location-available ?bound)) @@ -321,10 +314,10 @@ ;; Clear need-stereo so the planner won't try to use the stereo action ;; again after the user request is satisfied. - (at end (not (need-stereo ?robot ?order ?base ?bound ?run-number))) + (at end (not (need-stereo ?robot ?order ?base ?bound))) ;; Mark success - (at end (completed-stereo ?robot ?order ?base ?bound ?run-number)) + (at end (completed-stereo ?robot ?order ?base ?bound)) ) ) diff --git a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl index c9173ff5..ed3d17ee 100644 --- a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl @@ -6,7 +6,6 @@ {{ bays }} {{ berths }} - location {{ robots }} - robot {{ orders }} - order - run1 run2 run3 run4 run5 - run-number ) (:goal diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl index f1855d02..9264f13f 100644 --- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -12,19 +12,18 @@ jem_bay0 jem_bay1 jem_bay2 jem_bay3 jem_bay4 jem_bay5 jem_bay6 jem_bay7 jem_bay8 berth1 berth2 - location bumble honey - robot o0 o1 o2 o3 o4 o5 o6 o7 o8 o9 - order - run1 run2 run3 run4 run5 - run-number ) (:goal (and - (completed-panorama bumble o0 jem_bay4 run1) - (completed-panorama bumble o1 jem_bay3 run1) - (completed-panorama bumble o2 jem_bay2 run1) - (completed-panorama bumble o3 jem_bay1 run1) - (completed-stereo bumble o4 jem_bay1 jem_bay4 run1) - (completed-panorama honey o0 jem_bay7 run1) - (completed-panorama honey o1 jem_bay6 run1) - (completed-panorama honey o2 jem_bay5 run1) + (completed-panorama bumble o0 jem_bay4) + (completed-panorama bumble o1 jem_bay3) + (completed-panorama bumble o2 jem_bay2) + (completed-panorama bumble o3 jem_bay1) + (completed-stereo bumble o4 jem_bay1 jem_bay4) + (completed-panorama honey o0 jem_bay7) + (completed-panorama honey o1 jem_bay6) + (completed-panorama honey o2 jem_bay5) (robot-at honey berth2) ) ) @@ -154,7 +153,7 @@ ;; need-stereo predicates must be asserted with identical parameters to the ;; stereo-completed goals. See the need-stereo docs for more. - (need-stereo bumble o4 jem_bay1 jem_bay4 run1) + (need-stereo bumble o4 jem_bay1 jem_bay4) ;; === Static numeric fluents === (= (order-identity o0) 0) diff --git a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py index ce98e096..782f8633 100755 --- a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py @@ -110,20 +110,17 @@ def yaml_action_from_pddl(action: str, static_config: YamlMapping) -> YamlMappin } if action_type == "panorama": - robot, _order, location, run_name = action_args[1:] - run_number = int(run_name[-1]) + robot, _order, location = action_args[1:] # Can discard order. Look up coordinates for location. return { "type": "panorama", "robot": robot, "location_name": location, "location_pos": static_config["bays"][location], - "run": run_number, } if action_type == "stereo": - robot, _order, base, bound, _check1, _check2, run_name = action_args[1:] - run_number = int(run_name[-1]) + robot, _order, base, bound, _check1, _check2 = action_args[1:] # Use base and bound to look up trajectory. traj_matches = [ traj @@ -135,7 +132,7 @@ def yaml_action_from_pddl(action: str, static_config: YamlMapping) -> YamlMappin ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" fplan = traj_matches[0]["fplan"] # Can discard order, base, bound, check1, check2. - return {"type": "stereo", "robot": robot, "fplan": fplan, "run": run_number} + return {"type": "stereo", "robot": robot, "fplan": fplan} assert False, "Never reach this point." return {} # Make pylint happy diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py index 035107e2..43e414da 100755 --- a/astrobee/survey_manager/survey_planner/tools/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -77,18 +77,16 @@ def pddl_goal_from_yaml(goal: YamlMapping, config_static: YamlMapping) -> str: robot = goal["robot"] order = goal["order"] location = goal["location"] - run = goal["run"] - return f"(completed-panorama {robot} o{order} {location} run{run})" + return f"(completed-panorama {robot} o{order} {location})" if goal_type == "stereo": robot = goal["robot"] order = goal["order"] trajectory = goal["trajectory"] - run = goal["run"] traj_info = config_static["stereo"][trajectory] base = traj_info["base_location"] bound = traj_info["bound_location"] - return f"(completed-stereo {robot} o{order} {base} {bound} run{run})" + return f"(completed-stereo {robot} o{order} {base} {bound})" if goal_type == "robot_at": robot = goal["robot"] @@ -175,7 +173,6 @@ def problem_generator( for config_path in config_paths: config.update(load_yaml(config_path)) params = {} - yaml.safe_dump(config, stream=sys.stdout) header_lines = ( ";; Auto-generated by problem_generator.py. Do not edit!\n" @@ -268,7 +265,6 @@ def problem_generator( config_text += config_path.read_text() params["config"] = comment_for_pddl(config_text) - # print(yaml.safe_dump(params, indent=4, sort_keys=False)) filled_template = TEMPLATE_SUBST_REGEX.sub(TemplateFiller(params), problem_template) output_path.write_text(filled_template) print(f"Wrote to {output_path}", file=sys.stderr) @@ -281,12 +277,10 @@ class CustomFormatter( def path_list(paths_text: str) -> List[pathlib.Path]: - cwd = pathlib.Path.cwd() return [pathlib.Path(pstr) for pstr in paths_text.split(",")] def main(): - cwd = pathlib.Path.cwd() parser = argparse.ArgumentParser( description=__doc__, formatter_class=CustomFormatter ) @@ -300,7 +294,7 @@ def main(): "--config", help="Comma-separated list of paths to YAML problem config inputs", type=path_list, - default=",".join([str(path) for path in DEFAULT_CONFIGS]) + default=",".join([str(path) for path in DEFAULT_CONFIGS]), ) parser.add_argument( "-o", From 574cb887b8c24cac28500ba61287aa89968368f9 Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Mon, 18 Dec 2023 19:27:59 -0500 Subject: [PATCH 03/11] Add support for PlanSys2 terminal output; remove num-orders from config --- .../data/jem_survey_static.yaml | 1 - .../pddl/jem_survey_template.pddl | 2 +- .../pddl/problem_jem_survey.pddl | 8 +- .../survey_planner/tools/problem_generator.py | 184 ++++++++++++++---- 4 files changed, 148 insertions(+), 47 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml index db617126..2a4e96ab 100644 --- a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml @@ -40,7 +40,6 @@ bays: bogus_bays: [jem_bay0, jem_bay8] berths: [berth1, berth2] robots: [bumble, honey] -num_orders: 10 stereo: # Meta-data about stereo survey options diff --git a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl index ed3d17ee..4f849544 100644 --- a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl @@ -24,7 +24,7 @@ {{ robots_different_predicates }} - {{ locations_different_predicates }} + {{ locs_different_predicates }} ;; === Dynamic predicates === {{ robot_available_predicates }} diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl index 9264f13f..bc29e751 100644 --- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -11,7 +11,7 @@ (:objects jem_bay0 jem_bay1 jem_bay2 jem_bay3 jem_bay4 jem_bay5 jem_bay6 jem_bay7 jem_bay8 berth1 berth2 - location bumble honey - robot - o0 o1 o2 o3 o4 o5 o6 o7 o8 o9 - order + o0 o1 o2 o3 o4 - order ) (:goal @@ -161,11 +161,6 @@ (= (order-identity o2) 2) (= (order-identity o3) 3) (= (order-identity o4) 4) - (= (order-identity o5) 5) - (= (order-identity o6) 6) - (= (order-identity o7) 7) - (= (order-identity o8) 8) - (= (order-identity o9) 9) ;; === Dynamic numeric fluents === (= (robot-order bumble) -1) @@ -218,7 +213,6 @@ ;; bogus_bays: [jem_bay0, jem_bay8] ;; berths: [berth1, berth2] ;; robots: [bumble, honey] -;; num_orders: 10 ;; ;; stereo: ;; # Meta-data about stereo survey options diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py index 43e414da..3071d4ad 100755 --- a/astrobee/survey_manager/survey_planner/tools/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -19,8 +19,15 @@ # under the License. """ -Generates a PDDL problem specification for the survey domain in domain_survey.pddl. Takes as input -a PDDL problem template and higher-level static and dynamic configuration specified in YAML. +Generates a problem specification for the survey domain in domain_survey.pddl. Output is either a +PDDL problem instance or the equivalent sequence of commands to pass to the PlanSys2 terminal (if +-t is specified). + +Takes as input a list of higher-level configuration files specified in YAML. Normally, this is a +static config that contains non-changing information like bay coordinates, plus a dynamic config +that contains things that are likely to change during execution, like initial state and +goals. However, the split is arbitrary and you can just concatenate all the config files to get the +same effect. The generator takes care of error-prone repetitive tasks like asserting predicates for which locations are different, which robots are different, asserting a need-stereo predicate for every @@ -29,6 +36,7 @@ """ import argparse +import io import itertools import pathlib import re @@ -159,11 +167,47 @@ def comment_for_pddl(text: str) -> str: return "\n".join([f";; {line}".strip() for line in text.splitlines()]) +class TerminalWriter: + def __init__(self): + self._buf = io.StringIO() + + def getvalue(self): + return self._buf.getvalue() + + def command(self, cmd: str) -> None: + self._buf.write(cmd + "\n") + + def set_instance(self, instance_name: str, pddl_type: str) -> None: + self.command(f"set instance {instance_name} {pddl_type}") + + def set_instances(self, instance_names: Iterable[str], pddl_type: str) -> None: + for instance_name in instance_names: + self.set_instance(instance_name, pddl_type) + + def set_predicate(self, predicate: str) -> None: + self.command(f"set predicate {predicate}") + + def set_predicates(self, predicates: Iterable[str]) -> None: + for predicate in predicates: + self.set_predicate(predicate) + + def set_goals(self, goals: Iterable[str]) -> None: + self.command(f"set goal (and {' '.join(goals)})") + + def set_fluent(self, fluent: str) -> None: + self.command(f"set function {fluent}") + + def set_fluents(self, fluents: Iterable[str]) -> None: + for fluent in fluents: + self.set_fluent(fluent) + + def problem_generator( problem_template_path: pathlib.Path, config_paths: Iterable[pathlib.Path], - output_path: pathlib.Path, + output_path_template: pathlib.Path, command: str, + terminal: bool, ) -> None: """ The main function that generates the problem. @@ -182,33 +226,59 @@ def problem_generator( ) for i, config_path in enumerate(config_paths): header_lines += f";; Config {i + 1}: {config_path}\n" - params["header"] = header_lines + if terminal: + writer = TerminalWriter() + else: + params["header"] = header_lines bays = list(config["bays"].keys()) bogus_bays = config["bogus_bays"] all_bays = sorted(bays + bogus_bays) - params["bays"] = " ".join(all_bays) + if terminal: + writer.set_instances(all_bays, "location") + else: + params["bays"] = " ".join(all_bays) berths = config["berths"] - params["berths"] = " ".join(berths) + if terminal: + writer.set_instances(berths, "location") + else: + params["berths"] = " ".join(berths) robots = config["robots"] - params["robots"] = " ".join(robots) - - num_orders = config["num_orders"] - params["orders"] = " ".join([f"o{i}" for i in range(num_orders)]) + if terminal: + writer.set_instances(robots, "robot") + else: + params["robots"] = " ".join(robots) + + max_order = max([goal.get("order", -1) for goal in config["goals"]]) + num_orders = max_order + 1 + orders = [f"o{i}" for i in range(num_orders)] + if terminal: + writer.set_instances(orders, "order") + else: + params["orders"] = " ".join(orders) yaml_goals = config["goals"] pddl_goals = [pddl_goal_from_yaml(goal, config) for goal in yaml_goals] - params["goals"] = indent_lines(pddl_goals, 12) + if terminal: + writer.set_goals(pddl_goals) + else: + params["goals"] = indent_lines(pddl_goals, 12) move_connected_lines = [ f"(move-connected {a} {b})" for a, b in both_ways(pairwise(all_bays)) ] - params["move_connected_predicates"] = indent_lines(move_connected_lines, 8) + if terminal: + writer.set_predicates(move_connected_lines) + else: + params["move_connected_predicates"] = indent_lines(move_connected_lines, 8) location_real_lines = [f"(location-real {bay})" for bay in bays] - params["location_real_predicates"] = indent_lines(location_real_lines, 8) + if terminal: + writer.set_predicates(location_real_lines) + else: + params["location_real_predicates"] = indent_lines(location_real_lines, 8) candidates = (("jem_bay7", "berth1"), ("jem_bay7", "berth2")) dock_connected_lines = [ @@ -216,28 +286,41 @@ def problem_generator( for bay, berth in candidates if bay in bays and berth in berths ] - params["dock_connected_predicates"] = indent_lines(dock_connected_lines, 8) + if terminal: + writer.set_predicates(dock_connected_lines) + else: + params["dock_connected_predicates"] = indent_lines(dock_connected_lines, 8) robots_different_lines = [ f"(robots-different {a} {b})" for a, b in distinct_pairs(robots) ] - params["robots_different_predicates"] = indent_lines(robots_different_lines, 8) + if terminal: + writer.set_predicates(robots_different_lines) + else: + params["robots_different_predicates"] = indent_lines(robots_different_lines, 8) - locations_different_lines = [ + locs_different_lines = [ f"(locations-different {a} {b})" for a, b in distinct_pairs(all_bays) ] - params["locations_different_predicates"] = indent_lines( - locations_different_lines, 8 - ) + if terminal: + writer.set_predicates(locs_different_lines) + else: + params["locs_different_predicates"] = indent_lines(locs_different_lines, 8) robot_available_lines = [f"(robot-available {robot})" for robot in robots] - params["robot_available_predicates"] = indent_lines(robot_available_lines, 8) + if terminal: + writer.set_predicates(robot_available_lines) + else: + params["robot_available_predicates"] = indent_lines(robot_available_lines, 8) init = config["init"] robot_at_lines = [ f"(robot-at {robot} {init[robot]['location']})" for robot in robots ] - params["robot_at_predicates"] = indent_lines(robot_at_lines, 8) + if terminal: + writer.set_predicates(robot_at_lines) + else: + params["robot_at_predicates"] = indent_lines(robot_at_lines, 8) all_locations = all_bays + berths occupied_locations = [init[robot]["location"] for robot in robots] @@ -245,29 +328,47 @@ def problem_generator( location_available_lines = [ f"(location-available {location})" for location in available_locations ] - params["location_available_predicates"] = indent_lines(location_available_lines, 8) + if terminal: + writer.set_predicates(location_available_lines) + else: + params["location_available_predicates"] = indent_lines(location_available_lines, 8) need_stereo_lines = [ goal.replace("completed-stereo", "need-stereo") for goal in pddl_goals if "completed-stereo" in goal ] - params["need_stereo_predicates"] = indent_lines(need_stereo_lines, 8) + if terminal: + writer.set_predicates(need_stereo_lines) + else: + params["need_stereo_predicates"] = indent_lines(need_stereo_lines, 8) order_identity_lines = [f"(= (order-identity o{i}) {i})" for i in range(num_orders)] - params["order_identity_fluents"] = indent_lines(order_identity_lines, 8) + if terminal: + writer.set_fluents(order_identity_lines) + else: + params["order_identity_fluents"] = indent_lines(order_identity_lines, 8) robot_order_lines = [f"(= (robot-order {robot}) -1)" for robot in robots] - params["robot_order_fluents"] = indent_lines(robot_order_lines, 8) - - config_text = "" - for config_path in config_paths: - config_text += config_path.read_text() - params["config"] = comment_for_pddl(config_text) - - filled_template = TEMPLATE_SUBST_REGEX.sub(TemplateFiller(params), problem_template) - output_path.write_text(filled_template) - print(f"Wrote to {output_path}", file=sys.stderr) + if terminal: + writer.set_fluents(robot_order_lines) + else: + params["robot_order_fluents"] = indent_lines(robot_order_lines, 8) + + if terminal: + output_path = pathlib.Path(str(output_path_template).replace("{ext}", ".ps2.pddl")) + output_path.write_text(writer.getvalue()) + print(f"Wrote to {output_path}", file=sys.stderr) + else: + config_text = "" + for config_path in config_paths: + config_text += config_path.read_text() + params["config"] = comment_for_pddl(config_text) + + filled_template = TEMPLATE_SUBST_REGEX.sub(TemplateFiller(params), problem_template) + output_path = pathlib.Path(str(output_path_template).replace("{ext}", ".pddl")) + output_path.write_text(filled_template) + print(f"Wrote to {output_path}", file=sys.stderr) class CustomFormatter( @@ -284,9 +385,15 @@ def main(): parser = argparse.ArgumentParser( description=__doc__, formatter_class=CustomFormatter ) + parser.add_argument( + "-t", "--terminal", + help="Format output for PlanSys2 terminal instead of PDDL", + action="store_true", + default=False, + ) parser.add_argument( "--problem-template", - help="Path to input PDDL problem template", + help="Path to input PDDL problem template (unused if -t specified)", type=pathlib.Path, default=PDDL_DIR / "jem_survey_template.pddl", ) @@ -299,17 +406,18 @@ def main(): parser.add_argument( "-o", "--output", - help="Path for output PDDL problem", + help="Path for output PDDL problem ({ext} filled with extension)", type=pathlib.Path, - default=PDDL_DIR / "problem_jem_survey.pddl", + default=PDDL_DIR / "problem_jem_survey{ext}", ) args = parser.parse_args() problem_generator( problem_template_path=args.problem_template, config_paths=args.config, - output_path=args.output, + output_path_template=args.output, command=shlex.join(sys.argv), + terminal=args.terminal, ) From 9dba17cc61766cc240de5faded062bdbdf407b18 Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Tue, 19 Dec 2023 11:03:01 -0500 Subject: [PATCH 04/11] Refactor problem_generator.py to more cleanly support two output formats --- .../pddl/jem_survey_template.pddl | 28 +- .../pddl/problem_jem_survey.pddl | 9 - .../pddl/problem_jem_survey.ps2.pddl | 139 +++++++++ .../survey_planner/tools/problem_generator.py | 289 +++++++++++------- 4 files changed, 316 insertions(+), 149 deletions(-) create mode 100644 astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.ps2.pddl diff --git a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl index 4f849544..8cc991f1 100644 --- a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl @@ -3,9 +3,7 @@ (:domain survey-manager) (:metric minimize (total-time)) (:objects - {{ bays }} {{ berths }} - location - {{ robots }} - robot - {{ orders }} - order + {{ objects }} ) (:goal @@ -16,32 +14,16 @@ (:init ;; === Static predicates === - {{ move_connected_predicates }} - - {{ location_real_predicates }} - - {{ dock_connected_predicates }} - - {{ robots_different_predicates }} - - {{ locs_different_predicates }} + {{ static_predicates }} ;; === Dynamic predicates === - {{ robot_available_predicates }} - - {{ robot_at_predicates }} - - {{ location_available_predicates }} - - ;; need-stereo predicates must be asserted with identical parameters to the - ;; stereo-completed goals. See the need-stereo docs for more. - {{ need_stereo_predicates }} + {{ dynamic_predicates }} ;; === Static numeric fluents === - {{ order_identity_fluents }} + {{ static_fluents }} ;; === Dynamic numeric fluents === - {{ robot_order_fluents }} + {{ dynamic_fluents }} ) ;; end :init ) ;; end problem diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl index bc29e751..3314e7f1 100644 --- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -46,7 +46,6 @@ (move-connected jem_bay7 jem_bay6) (move-connected jem_bay7 jem_bay8) (move-connected jem_bay8 jem_bay7) - (location-real jem_bay1) (location-real jem_bay2) (location-real jem_bay3) @@ -54,13 +53,10 @@ (location-real jem_bay5) (location-real jem_bay6) (location-real jem_bay7) - (dock-connected jem_bay7 berth1) (dock-connected jem_bay7 berth2) - (robots-different bumble honey) (robots-different honey bumble) - (locations-different jem_bay0 jem_bay1) (locations-different jem_bay0 jem_bay2) (locations-different jem_bay0 jem_bay3) @@ -137,10 +133,8 @@ ;; === Dynamic predicates === (robot-available bumble) (robot-available honey) - (robot-at bumble berth1) (robot-at honey berth2) - (location-available jem_bay0) (location-available jem_bay1) (location-available jem_bay2) @@ -150,9 +144,6 @@ (location-available jem_bay6) (location-available jem_bay7) (location-available jem_bay8) - - ;; need-stereo predicates must be asserted with identical parameters to the - ;; stereo-completed goals. See the need-stereo docs for more. (need-stereo bumble o4 jem_bay1 jem_bay4) ;; === Static numeric fluents === diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.ps2.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.ps2.pddl new file mode 100644 index 00000000..f39e5694 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.ps2.pddl @@ -0,0 +1,139 @@ +set instance jem_bay0 location +set instance jem_bay1 location +set instance jem_bay2 location +set instance jem_bay3 location +set instance jem_bay4 location +set instance jem_bay5 location +set instance jem_bay6 location +set instance jem_bay7 location +set instance jem_bay8 location +set instance berth1 location +set instance berth2 location +set instance bumble robot +set instance honey robot +set instance o0 order +set instance o1 order +set instance o2 order +set instance o3 order +set instance o4 order +set goal (and (completed-panorama bumble o0 jem_bay4) (completed-panorama bumble o1 jem_bay3) (completed-panorama bumble o2 jem_bay2) (completed-panorama bumble o3 jem_bay1) (completed-stereo bumble o4 jem_bay1 jem_bay4) (completed-panorama honey o0 jem_bay7) (completed-panorama honey o1 jem_bay6) (completed-panorama honey o2 jem_bay5) (robot-at honey berth2)) +set predicate (move-connected jem_bay0 jem_bay1) +set predicate (move-connected jem_bay1 jem_bay0) +set predicate (move-connected jem_bay1 jem_bay2) +set predicate (move-connected jem_bay2 jem_bay1) +set predicate (move-connected jem_bay2 jem_bay3) +set predicate (move-connected jem_bay3 jem_bay2) +set predicate (move-connected jem_bay3 jem_bay4) +set predicate (move-connected jem_bay4 jem_bay3) +set predicate (move-connected jem_bay4 jem_bay5) +set predicate (move-connected jem_bay5 jem_bay4) +set predicate (move-connected jem_bay5 jem_bay6) +set predicate (move-connected jem_bay6 jem_bay5) +set predicate (move-connected jem_bay6 jem_bay7) +set predicate (move-connected jem_bay7 jem_bay6) +set predicate (move-connected jem_bay7 jem_bay8) +set predicate (move-connected jem_bay8 jem_bay7) +set predicate (location-real jem_bay1) +set predicate (location-real jem_bay2) +set predicate (location-real jem_bay3) +set predicate (location-real jem_bay4) +set predicate (location-real jem_bay5) +set predicate (location-real jem_bay6) +set predicate (location-real jem_bay7) +set predicate (dock-connected jem_bay7 berth1) +set predicate (dock-connected jem_bay7 berth2) +set predicate (robots-different bumble honey) +set predicate (robots-different honey bumble) +set predicate (locations-different jem_bay0 jem_bay1) +set predicate (locations-different jem_bay0 jem_bay2) +set predicate (locations-different jem_bay0 jem_bay3) +set predicate (locations-different jem_bay0 jem_bay4) +set predicate (locations-different jem_bay0 jem_bay5) +set predicate (locations-different jem_bay0 jem_bay6) +set predicate (locations-different jem_bay0 jem_bay7) +set predicate (locations-different jem_bay0 jem_bay8) +set predicate (locations-different jem_bay1 jem_bay0) +set predicate (locations-different jem_bay1 jem_bay2) +set predicate (locations-different jem_bay1 jem_bay3) +set predicate (locations-different jem_bay1 jem_bay4) +set predicate (locations-different jem_bay1 jem_bay5) +set predicate (locations-different jem_bay1 jem_bay6) +set predicate (locations-different jem_bay1 jem_bay7) +set predicate (locations-different jem_bay1 jem_bay8) +set predicate (locations-different jem_bay2 jem_bay0) +set predicate (locations-different jem_bay2 jem_bay1) +set predicate (locations-different jem_bay2 jem_bay3) +set predicate (locations-different jem_bay2 jem_bay4) +set predicate (locations-different jem_bay2 jem_bay5) +set predicate (locations-different jem_bay2 jem_bay6) +set predicate (locations-different jem_bay2 jem_bay7) +set predicate (locations-different jem_bay2 jem_bay8) +set predicate (locations-different jem_bay3 jem_bay0) +set predicate (locations-different jem_bay3 jem_bay1) +set predicate (locations-different jem_bay3 jem_bay2) +set predicate (locations-different jem_bay3 jem_bay4) +set predicate (locations-different jem_bay3 jem_bay5) +set predicate (locations-different jem_bay3 jem_bay6) +set predicate (locations-different jem_bay3 jem_bay7) +set predicate (locations-different jem_bay3 jem_bay8) +set predicate (locations-different jem_bay4 jem_bay0) +set predicate (locations-different jem_bay4 jem_bay1) +set predicate (locations-different jem_bay4 jem_bay2) +set predicate (locations-different jem_bay4 jem_bay3) +set predicate (locations-different jem_bay4 jem_bay5) +set predicate (locations-different jem_bay4 jem_bay6) +set predicate (locations-different jem_bay4 jem_bay7) +set predicate (locations-different jem_bay4 jem_bay8) +set predicate (locations-different jem_bay5 jem_bay0) +set predicate (locations-different jem_bay5 jem_bay1) +set predicate (locations-different jem_bay5 jem_bay2) +set predicate (locations-different jem_bay5 jem_bay3) +set predicate (locations-different jem_bay5 jem_bay4) +set predicate (locations-different jem_bay5 jem_bay6) +set predicate (locations-different jem_bay5 jem_bay7) +set predicate (locations-different jem_bay5 jem_bay8) +set predicate (locations-different jem_bay6 jem_bay0) +set predicate (locations-different jem_bay6 jem_bay1) +set predicate (locations-different jem_bay6 jem_bay2) +set predicate (locations-different jem_bay6 jem_bay3) +set predicate (locations-different jem_bay6 jem_bay4) +set predicate (locations-different jem_bay6 jem_bay5) +set predicate (locations-different jem_bay6 jem_bay7) +set predicate (locations-different jem_bay6 jem_bay8) +set predicate (locations-different jem_bay7 jem_bay0) +set predicate (locations-different jem_bay7 jem_bay1) +set predicate (locations-different jem_bay7 jem_bay2) +set predicate (locations-different jem_bay7 jem_bay3) +set predicate (locations-different jem_bay7 jem_bay4) +set predicate (locations-different jem_bay7 jem_bay5) +set predicate (locations-different jem_bay7 jem_bay6) +set predicate (locations-different jem_bay7 jem_bay8) +set predicate (locations-different jem_bay8 jem_bay0) +set predicate (locations-different jem_bay8 jem_bay1) +set predicate (locations-different jem_bay8 jem_bay2) +set predicate (locations-different jem_bay8 jem_bay3) +set predicate (locations-different jem_bay8 jem_bay4) +set predicate (locations-different jem_bay8 jem_bay5) +set predicate (locations-different jem_bay8 jem_bay6) +set predicate (locations-different jem_bay8 jem_bay7) +set predicate (robot-available bumble) +set predicate (robot-available honey) +set predicate (robot-at bumble berth1) +set predicate (robot-at honey berth2) +set predicate (location-available jem_bay0) +set predicate (location-available jem_bay1) +set predicate (location-available jem_bay2) +set predicate (location-available jem_bay3) +set predicate (location-available jem_bay4) +set predicate (location-available jem_bay5) +set predicate (location-available jem_bay6) +set predicate (location-available jem_bay7) +set predicate (location-available jem_bay8) +set predicate (need-stereo bumble o4 jem_bay1 jem_bay4) +set function (= (order-identity o0) 0) +set function (= (order-identity o1) 1) +set function (= (order-identity o2) 2) +set function (= (order-identity o3) 3) +set function (= (order-identity o4) 4) +set function (= (robot-order bumble) -1) +set function (= (robot-order honey) -1) diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py index 3071d4ad..141f6a36 100755 --- a/astrobee/survey_manager/survey_planner/tools/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -36,13 +36,16 @@ """ import argparse +import collections import io import itertools +import os import pathlib import re import shlex import sys -from typing import Any, Dict, Iterable, List, Sequence, T, Tuple +from abc import ABC, abstractmethod +from typing import Any, Dict, Iterable, List, Mapping, Sequence, Tuple, TypeVar import yaml @@ -50,8 +53,8 @@ THIS_DIR = pathlib.Path(__file__).resolve().parent CWD = pathlib.Path.cwd() -DATA_DIR = (THIS_DIR / ".." / "data").resolve().relative_to(CWD) -PDDL_DIR = (THIS_DIR / ".." / "pddl").resolve().relative_to(CWD) +DATA_DIR = pathlib.Path(os.path.relpath(str((THIS_DIR / ".." / "data").resolve()), CWD)) +PDDL_DIR = pathlib.Path(os.path.relpath(str((THIS_DIR / ".." / "pddl").resolve()), CWD)) DEFAULT_CONFIGS = [ DATA_DIR / "jem_survey_static.yaml", DATA_DIR / "jem_survey_dynamic.yaml", @@ -59,6 +62,7 @@ # Type alias YamlMapping = Dict[str, Any] +T = TypeVar("T") # Replace the text "{{ foo }}" in the template with the value of the foo parameter TEMPLATE_SUBST_REGEX = re.compile(r"{{\s*([\w]+)\s*}}") @@ -151,7 +155,7 @@ class TemplateFiller: def __init__(self, params): self.params = params - def __call__(self, match: re.match) -> str: + def __call__(self, match: re.Match) -> str: param = match.group(1) if param not in self.params: raise KeyError( @@ -167,39 +171,142 @@ def comment_for_pddl(text: str) -> str: return "\n".join([f";; {line}".strip() for line in text.splitlines()]) -class TerminalWriter: - def __init__(self): - self._buf = io.StringIO() +class ProblemWriter(ABC): + "Abstract class for writing a problem intance." - def getvalue(self): - return self._buf.getvalue() + @abstractmethod + def getvalue(self) -> str: + "Return problem text to output." + + def set_param(self, key: str, value: str) -> None: + "Set template parameter `key` to `value`. (This method is a no-op; override in children.)" - def command(self, cmd: str) -> None: - self._buf.write(cmd + "\n") + @abstractmethod + def get_extension(self) -> str: + "Return standard extension to use for output file." - def set_instance(self, instance_name: str, pddl_type: str) -> None: - self.command(f"set instance {instance_name} {pddl_type}") + @abstractmethod + def declare_instance(self, instance_name: str, pddl_type: str) -> None: + "Declare `instance_name` as an instance of `pddl_type`." - def set_instances(self, instance_names: Iterable[str], pddl_type: str) -> None: + def declare_instances(self, instance_names: Iterable[str], pddl_type: str) -> None: + "Declare `instance_names` as instances of `pddl_type`." for instance_name in instance_names: - self.set_instance(instance_name, pddl_type) + self.declare_instance(instance_name, pddl_type) - def set_predicate(self, predicate: str) -> None: - self.command(f"set predicate {predicate}") + @abstractmethod + def declare_predicate(self, predicate: str, section: str) -> None: + "Declare `predicate` as a PDDL predicate in `section`." - def set_predicates(self, predicates: Iterable[str]) -> None: + def declare_predicates(self, predicates: Iterable[str], section: str) -> None: + "Declare `predicates` as PDDL predicates in `section`." for predicate in predicates: - self.set_predicate(predicate) + self.declare_predicate(predicate, section) - def set_goals(self, goals: Iterable[str]) -> None: - self.command(f"set goal (and {' '.join(goals)})") + @abstractmethod + def declare_goals(self, goals: Iterable[str]) -> None: + "Declare `goals` as PDDL goals. (Can only be called once, with all goals.)" - def set_fluent(self, fluent: str) -> None: - self.command(f"set function {fluent}") + @abstractmethod + def declare_fluent(self, fluent: str, section: str) -> None: + "Declare `fluent` as a PDDL fluent in `section`." - def set_fluents(self, fluents: Iterable[str]) -> None: + def declare_fluents(self, fluents: Iterable[str], section: str) -> None: + "Declare `fluents` as PDDL fluents in `section`." for fluent in fluents: - self.set_fluent(fluent) + self.declare_fluent(fluent, section) + + +class PddlWriter(ProblemWriter): + "Class for writing a problem intance in PDDL format." + + def __init__(self, template_path: pathlib.Path): + self.template_path = template_path + self._params: Dict[str, str] = {} + self._section_bufs: Mapping[str, io.StringIO] = collections.defaultdict( + io.StringIO + ) + self._instances_of_types: Mapping[str, List[str]] = collections.defaultdict( + list + ) + + def get_extension(self) -> str: + return ".pddl" + + def set_param(self, key: str, value: str) -> None: + "Set template parameter `key` to `value`." + self._params[key] = value + + def _get_objects_str(self) -> str: + "Return object instance declarations in PDDL format." + indent = " " * 8 + return "\n".join( + ( + f"{indent}{' '.join(instances)} - {pddl_type}" + for pddl_type, instances in self._instances_of_types.items() + ) + ).lstrip() + + def getvalue(self) -> str: + self._params["objects"] = self._get_objects_str() + self._params.update( + { + section: buf.getvalue().strip() + for section, buf in self._section_bufs.items() + } + ) + template_text = self.template_path.read_text() + filled_template = TEMPLATE_SUBST_REGEX.sub( + TemplateFiller(self._params), template_text + ) + return filled_template + + def declare(self, statement: str, section: str) -> None: + "Declare `statement` in `section`." + indent = " " * 8 + self._section_bufs[section].write(indent + statement + "\n") + + def declare_instance(self, instance_name: str, pddl_type: str) -> None: + self._instances_of_types[pddl_type].append(instance_name) + + def declare_predicate(self, predicate: str, section: str) -> None: + self.declare(predicate, section) + + def declare_goals(self, goals: Iterable[str]) -> None: + indent = " " * 12 + self.set_param("goals", "\n".join((indent + goal for goal in goals)).lstrip()) + + def declare_fluent(self, fluent: str, section: str) -> None: + self.declare(fluent, section) + + +class TerminalWriter(ProblemWriter): + "Class for writing a problem instance as a sequence of PlanSys2 terminal commands." + + def __init__(self): + self._buf = io.StringIO() + + def get_extension(self) -> str: + return ".ps2.pddl" + + def getvalue(self) -> str: + return self._buf.getvalue() + + def declare(self, statement: str) -> None: + "Declare `statement`." + self._buf.write(statement + "\n") + + def declare_instance(self, instance_name: str, pddl_type: str) -> None: + self.declare(f"set instance {instance_name} {pddl_type}") + + def declare_predicate(self, predicate: str, section: str) -> None: + self.declare(f"set predicate {predicate}") + + def declare_goals(self, goals: Iterable[str]) -> None: + self.declare(f"set goal (and {' '.join(goals)})") + + def declare_fluent(self, fluent: str, section: str) -> None: + self.declare(f"set function {fluent}") def problem_generator( @@ -212,73 +319,54 @@ def problem_generator( """ The main function that generates the problem. """ - problem_template = problem_template_path.read_text() config = {} for config_path in config_paths: config.update(load_yaml(config_path)) - params = {} - header_lines = ( - ";; Auto-generated by problem_generator.py. Do not edit!\n" - f";; Command was: {command}\n" - f";; Working directory was: {CWD}\n" - f";; Problem template: {problem_template_path}\n" - ) - for i, config_path in enumerate(config_paths): - header_lines += f";; Config {i + 1}: {config_path}\n" if terminal: - writer = TerminalWriter() + writer: ProblemWriter = TerminalWriter() else: - params["header"] = header_lines + writer = PddlWriter(problem_template_path) + header_lines = ( + ";; Auto-generated by problem_generator.py. Do not edit!\n" + f";; Command was: {command}\n" + f";; Working directory was: {CWD}\n" + f";; Problem template: {problem_template_path}\n" + ) + full_config = "" + for i, config_path in enumerate(config_paths): + header_lines += f";; Config {i + 1}: {config_path}\n" + full_config += config_path.read_text() + writer.set_param("header", header_lines) + writer.set_param("config", comment_for_pddl(full_config)) bays = list(config["bays"].keys()) bogus_bays = config["bogus_bays"] all_bays = sorted(bays + bogus_bays) - if terminal: - writer.set_instances(all_bays, "location") - else: - params["bays"] = " ".join(all_bays) + writer.declare_instances(all_bays, "location") berths = config["berths"] - if terminal: - writer.set_instances(berths, "location") - else: - params["berths"] = " ".join(berths) + writer.declare_instances(berths, "location") robots = config["robots"] - if terminal: - writer.set_instances(robots, "robot") - else: - params["robots"] = " ".join(robots) + writer.declare_instances(robots, "robot") max_order = max([goal.get("order", -1) for goal in config["goals"]]) num_orders = max_order + 1 orders = [f"o{i}" for i in range(num_orders)] - if terminal: - writer.set_instances(orders, "order") - else: - params["orders"] = " ".join(orders) + writer.declare_instances(orders, "order") yaml_goals = config["goals"] pddl_goals = [pddl_goal_from_yaml(goal, config) for goal in yaml_goals] - if terminal: - writer.set_goals(pddl_goals) - else: - params["goals"] = indent_lines(pddl_goals, 12) + writer.declare_goals(pddl_goals) move_connected_lines = [ f"(move-connected {a} {b})" for a, b in both_ways(pairwise(all_bays)) ] - if terminal: - writer.set_predicates(move_connected_lines) - else: - params["move_connected_predicates"] = indent_lines(move_connected_lines, 8) + writer.declare_predicates(move_connected_lines, "static_predicates") location_real_lines = [f"(location-real {bay})" for bay in bays] - if terminal: - writer.set_predicates(location_real_lines) - else: - params["location_real_predicates"] = indent_lines(location_real_lines, 8) + writer.declare_predicates(location_real_lines, "static_predicates") candidates = (("jem_bay7", "berth1"), ("jem_bay7", "berth2")) dock_connected_lines = [ @@ -286,41 +374,26 @@ def problem_generator( for bay, berth in candidates if bay in bays and berth in berths ] - if terminal: - writer.set_predicates(dock_connected_lines) - else: - params["dock_connected_predicates"] = indent_lines(dock_connected_lines, 8) + writer.declare_predicates(dock_connected_lines, "static_predicates") robots_different_lines = [ f"(robots-different {a} {b})" for a, b in distinct_pairs(robots) ] - if terminal: - writer.set_predicates(robots_different_lines) - else: - params["robots_different_predicates"] = indent_lines(robots_different_lines, 8) + writer.declare_predicates(robots_different_lines, "static_predicates") locs_different_lines = [ f"(locations-different {a} {b})" for a, b in distinct_pairs(all_bays) ] - if terminal: - writer.set_predicates(locs_different_lines) - else: - params["locs_different_predicates"] = indent_lines(locs_different_lines, 8) + writer.declare_predicates(locs_different_lines, "static_predicates") robot_available_lines = [f"(robot-available {robot})" for robot in robots] - if terminal: - writer.set_predicates(robot_available_lines) - else: - params["robot_available_predicates"] = indent_lines(robot_available_lines, 8) + writer.declare_predicates(robot_available_lines, "dynamic_predicates") init = config["init"] robot_at_lines = [ f"(robot-at {robot} {init[robot]['location']})" for robot in robots ] - if terminal: - writer.set_predicates(robot_at_lines) - else: - params["robot_at_predicates"] = indent_lines(robot_at_lines, 8) + writer.declare_predicates(robot_at_lines, "dynamic_predicates") all_locations = all_bays + berths occupied_locations = [init[robot]["location"] for robot in robots] @@ -328,65 +401,47 @@ def problem_generator( location_available_lines = [ f"(location-available {location})" for location in available_locations ] - if terminal: - writer.set_predicates(location_available_lines) - else: - params["location_available_predicates"] = indent_lines(location_available_lines, 8) + writer.declare_predicates(location_available_lines, "dynamic_predicates") need_stereo_lines = [ goal.replace("completed-stereo", "need-stereo") for goal in pddl_goals if "completed-stereo" in goal ] - if terminal: - writer.set_predicates(need_stereo_lines) - else: - params["need_stereo_predicates"] = indent_lines(need_stereo_lines, 8) + writer.declare_predicates(need_stereo_lines, "dynamic_predicates") order_identity_lines = [f"(= (order-identity o{i}) {i})" for i in range(num_orders)] - if terminal: - writer.set_fluents(order_identity_lines) - else: - params["order_identity_fluents"] = indent_lines(order_identity_lines, 8) + writer.declare_fluents(order_identity_lines, "static_fluents") robot_order_lines = [f"(= (robot-order {robot}) -1)" for robot in robots] - if terminal: - writer.set_fluents(robot_order_lines) - else: - params["robot_order_fluents"] = indent_lines(robot_order_lines, 8) + writer.declare_fluents(robot_order_lines, "dynamic_fluents") - if terminal: - output_path = pathlib.Path(str(output_path_template).replace("{ext}", ".ps2.pddl")) - output_path.write_text(writer.getvalue()) - print(f"Wrote to {output_path}", file=sys.stderr) - else: - config_text = "" - for config_path in config_paths: - config_text += config_path.read_text() - params["config"] = comment_for_pddl(config_text) - - filled_template = TEMPLATE_SUBST_REGEX.sub(TemplateFiller(params), problem_template) - output_path = pathlib.Path(str(output_path_template).replace("{ext}", ".pddl")) - output_path.write_text(filled_template) - print(f"Wrote to {output_path}", file=sys.stderr) + output_path = pathlib.Path( + str(output_path_template).replace("{ext}", writer.get_extension()) + ) + output_path.write_text(writer.getvalue()) + print(f"Wrote to {output_path}", file=sys.stderr) class CustomFormatter( argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter ): - pass + "Custom formatter for argparse that combines mixins." def path_list(paths_text: str) -> List[pathlib.Path]: + "Return the list of paths parsed from comma-separated list `paths_text`." return [pathlib.Path(pstr) for pstr in paths_text.split(",")] def main(): + "Parse arguments and invoke problem_generator()." parser = argparse.ArgumentParser( description=__doc__, formatter_class=CustomFormatter ) parser.add_argument( - "-t", "--terminal", + "-t", + "--terminal", help="Format output for PlanSys2 terminal instead of PDDL", action="store_true", default=False, From 43d453eb38c5800ffae3b464769a7bc1e0feaba2 Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Tue, 19 Dec 2023 11:12:12 -0500 Subject: [PATCH 05/11] plan_interpreter.py works with default arguments again --- .../data/sample_output_plan.yaml | 50 ++++++++----------- .../survey_planner/tools/plan_interpreter.py | 44 ++++++++-------- 2 files changed, 43 insertions(+), 51 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml index d17b7e1c..f4b5d2fe 100644 --- a/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml @@ -47,8 +47,7 @@ - 11.0 - -7.0 - 4.8 - run: 1 - duration_seconds: '900.000' + duration_seconds: '780.000' - start_time_seconds: '100.004' action: type: panorama @@ -58,9 +57,8 @@ - 11.0 - -9.7 - 4.8 - run: 1 - duration_seconds: '900.000' -- start_time_seconds: '990.005' + duration_seconds: '780.000' +- start_time_seconds: '870.005' action: type: move robot: bumble @@ -70,7 +68,7 @@ - -6.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '1000.005' +- start_time_seconds: '880.005' action: type: move robot: honey @@ -80,7 +78,7 @@ - -9.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '1010.006' +- start_time_seconds: '890.006' action: type: panorama robot: bumble @@ -89,9 +87,8 @@ - 11.0 - -6.0 - 4.8 - run: 1 - duration_seconds: '900.000' -- start_time_seconds: '1020.006' + duration_seconds: '780.000' +- start_time_seconds: '900.006' action: type: panorama robot: honey @@ -100,9 +97,8 @@ - 11.0 - -9.0 - 4.8 - run: 1 - duration_seconds: '900.000' -- start_time_seconds: '1910.007' + duration_seconds: '780.000' +- start_time_seconds: '1670.007' action: type: move robot: bumble @@ -112,7 +108,7 @@ - -5.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '1930.008' +- start_time_seconds: '1690.008' action: type: panorama robot: bumble @@ -121,9 +117,8 @@ - 11.0 - -5.0 - 4.8 - run: 1 - duration_seconds: '900.000' -- start_time_seconds: '2830.009' + duration_seconds: '780.000' +- start_time_seconds: '2470.009' action: type: move robot: bumble @@ -133,7 +128,7 @@ - -4.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '2850.010' +- start_time_seconds: '2490.010' action: type: panorama robot: bumble @@ -142,16 +137,14 @@ - 11.0 - -4.0 - 4.8 - run: 1 - duration_seconds: '900.000' -- start_time_seconds: '3750.011' + duration_seconds: '780.000' +- start_time_seconds: '3270.011' action: type: stereo robot: bumble fplan: jem_stereo_mapping_bay1_to_bay3.fplan - run: 1 duration_seconds: '600.000' -- start_time_seconds: '4350.012' +- start_time_seconds: '3870.012' action: type: move robot: honey @@ -161,7 +154,7 @@ - -8.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '4370.013' +- start_time_seconds: '3890.013' action: type: panorama robot: honey @@ -170,9 +163,8 @@ - 11.0 - -8.0 - 4.8 - run: 1 - duration_seconds: '900.000' -- start_time_seconds: '5270.014' + duration_seconds: '780.000' +- start_time_seconds: '4670.014' action: type: move robot: honey @@ -182,7 +174,7 @@ - -9.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '5290.015' +- start_time_seconds: '4690.015' action: type: move robot: honey @@ -192,7 +184,7 @@ - -9.7 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '5310.016' +- start_time_seconds: '4710.016' action: type: dock robot: honey diff --git a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py index 782f8633..3d9e9374 100755 --- a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py @@ -34,10 +34,17 @@ import pathlib import re import sys -from typing import Any, Dict, List +from typing import Any, Dict, Iterable, List import yaml +from problem_generator import CWD, DATA_DIR, PDDL_DIR, load_yaml, path_list + +DEFAULT_CONFIGS = [ + DATA_DIR / "jem_survey_static.yaml", + # Dynamic config not needed for interpreting the plan +] + ACTION_TYPE_OPTIONS = ("dock", "undock", "move", "panorama", "stereo") @@ -54,14 +61,6 @@ ) -def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: - """ - Return the YAML parse result for the file at `yaml_path`. - """ - with yaml_path.open(encoding="utf-8") as yaml_stream: - return yaml.safe_load(yaml_stream) - - class PlanAction: """ Class representing one entry in the output plan sequence of a PDDL planner. @@ -187,7 +186,9 @@ def ignore_aliases(self, data): def plan_interpreter( - config_static_path: pathlib.Path, plan_path: pathlib.Path, output_path: pathlib.Path + config_paths: Iterable[pathlib.Path], + plan_path: pathlib.Path, + output_path: pathlib.Path, ) -> None: """ The main function that interprets an entire plan file. @@ -195,11 +196,12 @@ def plan_interpreter( However, if you are receiving one action at a time from an executor, you may prefer to import this module and call yaml_action_from_pddl() directly. """ - config_static = load_yaml(config_static_path) + config = {} + for config_path in config_paths: + config.update(load_yaml(config_path)) pddl_actions = parse_plan(plan_path) yaml_actions = [ - yaml_plan_action_from_pddl(plan_action, config_static) - for plan_action in pddl_actions + yaml_plan_action_from_pddl(plan_action, config) for plan_action in pddl_actions ] with output_path.open("w", encoding="utf-8") as output_stream: yaml.dump(yaml_actions, output_stream, Dumper=NoAliasDumper, sort_keys=False) @@ -217,30 +219,28 @@ def main(): description=__doc__, formatter_class=CustomFormatter ) parser.add_argument( - "--config-static", - help="Path to input static problem config YAML (module geometry, available stereo surveys, etc.)", - type=pathlib.Path, - default="jem_survey_static.yaml", + "--config", + help="Comma-separated list of paths to YAML problem config inputs (only static config needed)", + type=path_list, + default=DEFAULT_CONFIGS, ) parser.add_argument( "--plan", help="Path to input plan generated by PDDL planner (parser currently tuned for POPF idiosyncrasies)", type=pathlib.Path, - default="sample_output_plan.txt", + default=DATA_DIR / "sample_output_plan.txt", ) parser.add_argument( "-o", "--output", help="Path for output converted to YAML format", type=pathlib.Path, - default="sample_output_plan.yaml", + default=DATA_DIR / "sample_output_plan.yaml", ) args = parser.parse_args() plan_interpreter( - config_static_path=args.config_static, - plan_path=args.plan, - output_path=args.output, + config_paths=args.config, plan_path=args.plan, output_path=args.output ) From 4016f263b0bc5ecf6a22eac618076079b0cdb2e4 Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Tue, 19 Dec 2023 14:12:33 -0500 Subject: [PATCH 06/11] Update .isort.cfg using scripts/git/configure_isort_paths.sh --- .isort.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.isort.cfg b/.isort.cfg index 46f14eaf..88864386 100644 --- a/.isort.cfg +++ b/.isort.cfg @@ -15,4 +15,4 @@ # folder, you can auto-update it by running # scripts/git/configure_isort_paths.sh. -src_paths = analyst/workspace/scripts,anomaly/image/scripts,astrobee/behaviors/inspection/scripts,astrobee/simulation/acoustics_cam/src,dense_map/geometry_mapper/tools,isaac_msgs/isaac_msgs/test,pano/pano_stitch/scripts,pano/pano_view/scripts,scripts/git +src_paths = analyst/workspace/scripts,anomaly/gmm-change-detection,anomaly/gmm-change-detection/scripts/gmm,anomaly/image/scripts,astrobee/behaviors/inspection/scripts,astrobee/simulation/acoustics_cam/src,astrobee/survey_manager/survey_planner/tools,dense_map/geometry_mapper/tools,dense_map/volumetric_mapper/scripts,pano/pano_stitch/scripts,pano/pano_view/scripts,scripts/git From c7bc8ec6a85f20fdf82ea00434b2b7f6be230cfe Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Tue, 19 Dec 2023 14:34:50 -0500 Subject: [PATCH 07/11] jem_survey_dynamic.yaml: Remove unused run parameter. --- .../data/jem_survey_dynamic.yaml | 18 +++++++++--------- .../pddl/problem_jem_survey.pddl | 18 +++++++++--------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml index 680641ff..da42d56f 100644 --- a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml @@ -24,11 +24,11 @@ goals: -- {type: panorama, robot: bumble, order: 0, location: jem_bay4, run: 1} -- {type: panorama, robot: bumble, order: 1, location: jem_bay3, run: 1} -- {type: panorama, robot: bumble, order: 2, location: jem_bay2, run: 1} -- {type: panorama, robot: bumble, order: 3, location: jem_bay1, run: 1} -- {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3, run: 1} +- {type: panorama, robot: bumble, order: 0, location: jem_bay4} +- {type: panorama, robot: bumble, order: 1, location: jem_bay3} +- {type: panorama, robot: bumble, order: 2, location: jem_bay2} +- {type: panorama, robot: bumble, order: 3, location: jem_bay1} +- {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3} # We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to # get confused and greatly increase the total run time. For some reason, it doesn't notice it can @@ -37,13 +37,13 @@ goals: # about 2x. # - {type: robot_at, robot: bumble, location: berth1} -- {type: panorama, robot: honey, order: 0, location: jem_bay7, run: 1} -- {type: panorama, robot: honey, order: 1, location: jem_bay6, run: 1} -- {type: panorama, robot: honey, order: 2, location: jem_bay5, run: 1} +- {type: panorama, robot: honey, order: 0, location: jem_bay7} +- {type: panorama, robot: honey, order: 1, location: jem_bay6} +- {type: panorama, robot: honey, order: 2, location: jem_bay5} # This is another objective we want to include that for some reason causes POPF to fail to generate # a plan (hang indefinitely). No obvious reason why it should cause a problem. -# - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7, run: 1} +# - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7} - {type: robot_at, robot: honey, location: berth2} diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl index 3314e7f1..7557c669 100644 --- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -246,11 +246,11 @@ ;; ;; goals: ;; -;; - {type: panorama, robot: bumble, order: 0, location: jem_bay4, run: 1} -;; - {type: panorama, robot: bumble, order: 1, location: jem_bay3, run: 1} -;; - {type: panorama, robot: bumble, order: 2, location: jem_bay2, run: 1} -;; - {type: panorama, robot: bumble, order: 3, location: jem_bay1, run: 1} -;; - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3, run: 1} +;; - {type: panorama, robot: bumble, order: 0, location: jem_bay4} +;; - {type: panorama, robot: bumble, order: 1, location: jem_bay3} +;; - {type: panorama, robot: bumble, order: 2, location: jem_bay2} +;; - {type: panorama, robot: bumble, order: 3, location: jem_bay1} +;; - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3} ;; ;; # We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to ;; # get confused and greatly increase the total run time. For some reason, it doesn't notice it can @@ -259,13 +259,13 @@ ;; # about 2x. ;; # - {type: robot_at, robot: bumble, location: berth1} ;; -;; - {type: panorama, robot: honey, order: 0, location: jem_bay7, run: 1} -;; - {type: panorama, robot: honey, order: 1, location: jem_bay6, run: 1} -;; - {type: panorama, robot: honey, order: 2, location: jem_bay5, run: 1} +;; - {type: panorama, robot: honey, order: 0, location: jem_bay7} +;; - {type: panorama, robot: honey, order: 1, location: jem_bay6} +;; - {type: panorama, robot: honey, order: 2, location: jem_bay5} ;; ;; # This is another objective we want to include that for some reason causes POPF to fail to generate ;; # a plan (hang indefinitely). No obvious reason why it should cause a problem. -;; # - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7, run: 1} +;; # - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7} ;; ;; - {type: robot_at, robot: honey, location: berth2} ;; From 33e62c7eba0d6ebf675194eb6f501109f645a55f Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Tue, 19 Dec 2023 15:10:04 -0500 Subject: [PATCH 08/11] Fix legacy isort styling error in unrelated file so lint passes --- anomaly/gmm-change-detection/scripts/gmm/gmm_change_detection.py | 1 + 1 file changed, 1 insertion(+) diff --git a/anomaly/gmm-change-detection/scripts/gmm/gmm_change_detection.py b/anomaly/gmm-change-detection/scripts/gmm/gmm_change_detection.py index ab9d6bb7..b0e62348 100755 --- a/anomaly/gmm-change-detection/scripts/gmm/gmm_change_detection.py +++ b/anomaly/gmm-change-detection/scripts/gmm/gmm_change_detection.py @@ -23,6 +23,7 @@ import sys import numpy as np + from gmm.artificial_data import generate_data from gmm.gmm_edit import * from gmm.gmm_mml import GmmMml From c186b583c326fceac5983d4cf879f71692097bad Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Thu, 21 Dec 2023 14:06:09 -0500 Subject: [PATCH 09/11] Added let-other-robot-reach constraint so we can solve the full problem with POPF/OPTIC --- .../data/jem_survey_dynamic.yaml | 30 ++- .../data/jem_survey_static.yaml | 10 +- .../data/sample_output_plan.txt | 173 ++++++++++++++--- .../data/sample_output_plan.yaml | 183 ++++++++++++++++-- .../survey_planner/pddl/domain_survey.pddl | 41 ++++ .../pddl/problem_jem_survey.pddl | 21 +- .../survey_planner/tools/plan_interpreter.py | 182 +++++++++++++++-- .../survey_planner/tools/problem_generator.py | 8 +- 8 files changed, 557 insertions(+), 91 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml index da42d56f..316a3101 100644 --- a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml @@ -29,22 +29,20 @@ goals: - {type: panorama, robot: bumble, order: 2, location: jem_bay2} - {type: panorama, robot: bumble, order: 3, location: jem_bay1} - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3} - -# We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to -# get confused and greatly increase the total run time. For some reason, it doesn't notice it can -# use the same plan as without this goal and then add some motion actions at the end to achieve this -# goal. Instead, it falls back to only undocking one robot at a time, which slows things down by -# about 2x. -# - {type: robot_at, robot: bumble, location: berth1} - -- {type: panorama, robot: honey, order: 0, location: jem_bay7} -- {type: panorama, robot: honey, order: 1, location: jem_bay6} -- {type: panorama, robot: honey, order: 2, location: jem_bay5} - -# This is another objective we want to include that for some reason causes POPF to fail to generate -# a plan (hang indefinitely). No obvious reason why it should cause a problem. -# - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7} - +# This is one of the goals we previously had to comment out for POPF to return a halfway decent +# plan. Adding a let_other_robot_reach goal mostly fixed the problem. +- {type: robot_at, robot: bumble, location: berth1} + +# This let_other_robot_reach goal is effectively a very specific kind of between-robot ordering +# constraint. It tells honey to let bumble get to bay 5 before taking its first panorama. Without +# this constraint, POPF produces a very inefficient plan where bumble never leaves the dock until +# after honey finishes all its tasks and returns to dock. +- {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5} +- {type: panorama, robot: honey, order: 1, location: jem_bay7} +- {type: panorama, robot: honey, order: 2, location: jem_bay6} +- {type: panorama, robot: honey, order: 3, location: jem_bay5} +# This is the other objective we previously had to comment out for POPF to return a decent plan. +- {type: stereo, robot: honey, order: 4, trajectory: jem_bay4_to_bay7} - {type: robot_at, robot: honey, location: berth2} init: diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml index 2a4e96ab..fbfd7e02 100644 --- a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml @@ -44,14 +44,14 @@ robots: [bumble, honey] stereo: # Meta-data about stereo survey options jem_bay1_to_bay3: - # fplan: Name of external fplan specification of trajectory in astrobee_ops/gds/plans/ISAAC/ + # fplan: Name of external fplan specification of trajectory in astrobee_ops/gds/plans/ISAAC/ . The + # bay names are intended to indicate which bays are covered by the stereo survey. fplan: "jem_stereo_mapping_bay1_to_bay3.fplan" # base_location: Where trajectory starts and ends for planning purposes (rough location, not exact) base_location: jem_bay1 - # bound_location: The other end of the interval covered by the trajectory, for planner collision - # check purposes. (Note a trajectory may fly a bit into a bay that it doesn't claim to cover. - # The two surveys that cover the module purposefully overlap.) - bound_location: jem_bay4 + # bound_location: The other end of the interval visited by the trajectory, for planner collision + # check purposes. + bound_location: jem_bay4 # The survey flies into bay4 even though it only covers up to bay3 jem_bay4_to_bay7: fplan: "jem_stereo_mapping_bay4_to_bay7.fplan" base_location: jem_bay7 diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt index 03eb0bd5..b25de7b0 100644 --- a/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt @@ -1,31 +1,154 @@ -$ time popf pddl/domain_survey.pddl pddl/problem_jem_survey.pddl +$ (ulimit -t 10 && time ./optic_planner pddl/domain_survey.pddl pddl/problem_jem_survey.pddl) +Number of literals: 215 Constructing lookup tables: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] Post filtering unreachable actions: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] -16% of the ground temporal actions in this problem are compression-safe -b (25.000 | 30.000)b (24.000 | 50.001)b (23.000 | 70.002)b (22.000 | 90.003)b (21.000 | 870.004)b (20.000 | 870.004)b (19.000 | 890.005)b (18.000 | 1670.006)b (17.000 | 1670.006)b (16.000 | 1690.007)b (15.000 | 2470.008)b (14.000 | 2470.008)b (13.000 | 2490.009)b (12.000 | 3270.010)b (11.000 | 3270.010)b (10.000 | 3870.011)b (9.000 | 3870.011)b (8.000 | 3870.011)b (7.000 | 3870.011)b (6.000 | 3870.011)b (5.000 | 3870.011)b (4.000 | 4670.013)b (3.000 | 4670.013)b (2.000 | 4690.014)b (1.000 | 4710.015);;;; Solution Found -; Time 0.21 +(robot-order bumble) has finite bounds: [-1.000,4.000] +(robot-order honey) has finite bounds: [-1.000,4.000] +Have identified that smaller values of (robot-order bumble) are preferable +Have identified that smaller values of (robot-order honey) are preferable +Seeing if metric is defined in terms of task vars or a minimal value of makespan +- Yes it is +Recognised a monotonic-change-induced limit on -1.000* +- Must be >= the metric +Looking for achievers for goal index 0, fact (completed-panorama bumble o0 jem_bay4) with fID 83 + (panorama bumble o0 jem_bay4) +For limits: literal goal index 0, fact (completed-panorama bumble o0 jem_bay4), could be achieved by operator (panorama bumble o0 jem_bay4), which has other interesting effects (including one on (robot-available bumble) ) +Looking for achievers for goal index 1, fact (completed-panorama bumble o1 jem_bay3) with fID 75 + (panorama bumble o1 jem_bay3) +For limits: literal goal index 1, fact (completed-panorama bumble o1 jem_bay3), could be achieved by operator (panorama bumble o1 jem_bay3), which has other interesting effects (including one on (robot-available bumble) ) +Looking for achievers for goal index 2, fact (completed-panorama bumble o2 jem_bay2) with fID 67 + (panorama bumble o2 jem_bay2) +For limits: literal goal index 2, fact (completed-panorama bumble o2 jem_bay2), could be achieved by operator (panorama bumble o2 jem_bay2), which has other interesting effects (including one on (robot-available bumble) ) +Looking for achievers for goal index 3, fact (completed-panorama bumble o3 jem_bay1) with fID 59 + (panorama bumble o3 jem_bay1) +For limits: literal goal index 3, fact (completed-panorama bumble o3 jem_bay1), could be achieved by operator (panorama bumble o3 jem_bay1), which has other interesting effects (including one on (robot-available bumble) ) +Looking for achievers for goal index 4, fact (completed-stereo bumble o4 jem_bay1 jem_bay4) with fID 123 + (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) (stereo bumble o4 jem_bay1 jem_bay4 jem_bay3 jem_bay5) +For limits: literal goal index 4, fact (completed-stereo bumble o4 jem_bay1 jem_bay4), could be achieved by operator (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3), which has other interesting effects (including one on (location-available jem_bay4) ) +For limits: literal goal index 5, fact (robot-at bumble berth1), is static or a precondition +Looking for achievers for goal index 6, fact (completed-let-other-robot-reach honey o0 jem_bay5) with fID 155 + (let-other-robot-reach honey o0 jem_bay5 bumble) + Looking at numeric effects of (let-other-robot-reach honey o0 jem_bay5 bumble): 1 and 0 +For limits: literal goal index 6, fact (completed-let-other-robot-reach honey o0 jem_bay5), could be achieved by operator (let-other-robot-reach honey o0 jem_bay5 bumble), which has a non-trivial numeric effect ((robot-order honey) = 0.000) +Looking for achievers for goal index 7, fact (completed-panorama honey o1 jem_bay7) with fID 116 + (panorama honey o1 jem_bay7) +For limits: literal goal index 7, fact (completed-panorama honey o1 jem_bay7), could be achieved by operator (panorama honey o1 jem_bay7), which has other interesting effects (including one on (robot-available honey) ) +Looking for achievers for goal index 8, fact (completed-panorama honey o2 jem_bay6) with fID 108 + (panorama honey o2 jem_bay6) +For limits: literal goal index 8, fact (completed-panorama honey o2 jem_bay6), could be achieved by operator (panorama honey o2 jem_bay6), which has other interesting effects (including one on (robot-available honey) ) +Looking for achievers for goal index 9, fact (completed-panorama honey o3 jem_bay5) with fID 100 + (panorama honey o3 jem_bay5) +For limits: literal goal index 9, fact (completed-panorama honey o3 jem_bay5), could be achieved by operator (panorama honey o3 jem_bay5), which has other interesting effects (including one on (robot-available honey) ) +Looking for achievers for goal index 10, fact (completed-stereo honey o4 jem_bay7 jem_bay4) with fID 124 + (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) (stereo honey o4 jem_bay7 jem_bay4 jem_bay3 jem_bay5) +For limits: literal goal index 10, fact (completed-stereo honey o4 jem_bay7 jem_bay4), could be achieved by operator (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3), which has other interesting effects (including one on (location-available jem_bay4) ) +For limits: literal goal index 11, fact (robot-at honey berth2), is static or a precondition +Assignment numeric effect ((robot-order bumble) = 0.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 0.000) makes effects on 1 be order-dependent +Assignment numeric effect ((robot-order bumble) = 1.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 1.000) makes effects on 1 be order-dependent +Assignment numeric effect ((robot-order bumble) = 2.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 2.000) makes effects on 1 be order-dependent +Assignment numeric effect ((robot-order bumble) = 3.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 3.000) makes effects on 1 be order-dependent +Assignment numeric effect ((robot-order bumble) = 4.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 4.000) makes effects on 1 be order-dependent +27% of the ground temporal actions in this problem are compression-safe +Initial heuristic = 29.000, admissible cost estimate 930.008 +b (28.000 | 630.001) +Resorting to best-first search +Running WA* with W = 5.000, not restarting with goal states +b (28.000 | 630.001)b (28.000 | 70.003)b (27.000 | 870.005)b (26.000 | 870.005)b (25.000 | 880.005)b (24.000 | 880.005)b (23.000 | 1670.007)b (22.000 | 1670.007)b (21.000 | 1680.007)b (20.000 | 1680.007)b (19.000 | 2470.009)b (18.000 | 2470.009)b (17.000 | 2470.009)b (16.000 | 3270.011)b (15.000 | 3270.011)b (14.000 | 3870.012)b (13.000 | 3890.013)b (12.000 | 4470.013)b (12.000 | 3930.015)b (11.000 | 3950.016)b (10.000 | 3970.017)b (9.000 | 3990.018)b (7.000 | 4020.019)b (6.000 | 4650.021)b (4.000 | 4890.024)b (3.000 | 4910.025)b (2.000 | 5510.026)b (1.000 | 5510.026)(G) +; LP calculated the cost + +; Plan found with metric 5540.027 +; Theoretical reachable cost 5540.028 +; States evaluated so far: 504 +; States pruned based on pre-heuristic cost lower bound: 0 +; Time 1.64 0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) [30.000] 30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] 50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] -70.003: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] -70.003: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] -90.004: (panorama bumble o0 jem_bay4) [780.000] -100.004: (panorama honey o0 jem_bay7) [780.000] -870.005: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] -880.005: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] -890.006: (panorama bumble o1 jem_bay3) [780.000] -900.006: (panorama honey o1 jem_bay6) [780.000] -1670.007: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] -1690.008: (panorama bumble o2 jem_bay2) [780.000] -2470.009: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] -2490.010: (panorama bumble o3 jem_bay1) [780.000] -3270.011: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) [600.000] -3870.012: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] -3890.013: (panorama honey o2 jem_bay5) [780.000] -4670.014: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] -4690.015: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] -4710.016: (dock honey jem_bay7 berth2) [30.000] +70.003: (let-other-robot-reach honey o0 jem_bay5 bumble) [0.000] +70.004: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] +70.004: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +90.005: (panorama bumble o0 jem_bay4) [780.000] +100.005: (panorama honey o1 jem_bay7) [780.000] +870.006: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +880.006: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +890.007: (panorama bumble o1 jem_bay3) [780.000] +900.007: (panorama honey o2 jem_bay6) [780.000] +1670.008: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +1680.008: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +1690.009: (panorama bumble o2 jem_bay2) [780.000] +1700.009: (dock honey jem_bay7 berth2) [30.000] +2470.010: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] +2490.011: (panorama bumble o3 jem_bay1) [780.000] +3270.012: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) [600.000] +3870.013: (move bumble jem_bay1 jem_bay2 jem_bay3) [20.000] +3890.014: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] +3910.015: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] +3930.016: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000] +3950.017: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000] +3970.018: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000] +3990.019: (dock bumble jem_bay7 berth1) [30.000] +4020.020: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +4050.021: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +4070.022: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] +4090.023: (panorama honey o3 jem_bay5) [780.000] +4870.024: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] +4890.025: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +4910.026: (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) [600.000] +5510.027: (dock honey jem_bay7 berth2) [30.000] + + * All goal deadlines now no later than 5540.027 +b (1.000 | 5470.024)(G) +; LP calculated the cost + +; Plan found with metric 5500.025 +; Theoretical reachable cost 5500.026 +; States evaluated so far: 866 +; States pruned based on pre-heuristic cost lower bound: 1 +; Time 2.85 +0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) [30.000] +30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] +50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] +70.003: (let-other-robot-reach honey o0 jem_bay5 bumble) [0.000] +70.004: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] +70.004: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +90.005: (panorama bumble o0 jem_bay4) [780.000] +100.005: (panorama honey o1 jem_bay7) [780.000] +870.006: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +880.006: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +890.007: (panorama bumble o1 jem_bay3) [780.000] +900.007: (panorama honey o2 jem_bay6) [780.000] +1670.008: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +1690.009: (panorama bumble o2 jem_bay2) [780.000] +2470.010: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] +2490.011: (panorama bumble o3 jem_bay1) [780.000] +3270.012: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) [600.000] +3870.013: (move bumble jem_bay1 jem_bay2 jem_bay3) [20.000] +3870.013: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] +3890.014: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] +3890.014: (panorama honey o3 jem_bay5) [780.000] +4670.015: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] +4690.016: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] +4690.016: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +4710.017: (dock honey jem_bay7 berth2) [30.000] +4710.017: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +4730.018: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +4740.018: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +4770.019: (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) [600.000] +5370.020: (dock honey jem_bay7 berth2) [30.000] +5370.020: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] +5390.021: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] +5410.022: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000] +5430.023: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000] +5450.024: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000] +5470.025: (dock bumble jem_bay7 berth1) [30.000] + + * All goal deadlines now no later than 5500.025 -real 0m0.221s -user 0m0.208s -sys 0m0.013s +real 0m10.009s +user 0m9.767s +sys 0m0.240s diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml index f4b5d2fe..7c624cb1 100644 --- a/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml @@ -7,6 +7,7 @@ action: type: move robot: bumble + from_name: jem_bay7 to_name: jem_bay6 to_pos: - 11.0 @@ -17,28 +18,30 @@ action: type: move robot: bumble + from_name: jem_bay6 to_name: jem_bay5 to_pos: - 11.0 - -8.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '70.003' +- start_time_seconds: '70.004' action: type: move robot: bumble + from_name: jem_bay5 to_name: jem_bay4 to_pos: - 11.0 - -7.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '70.003' +- start_time_seconds: '70.004' action: type: undock robot: honey duration_seconds: '30.000' -- start_time_seconds: '90.004' +- start_time_seconds: '90.005' action: type: panorama robot: bumble @@ -48,7 +51,7 @@ - -7.0 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '100.004' +- start_time_seconds: '100.005' action: type: panorama robot: honey @@ -58,27 +61,29 @@ - -9.7 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '870.005' +- start_time_seconds: '870.006' action: type: move robot: bumble + from_name: jem_bay4 to_name: jem_bay3 to_pos: - 11.0 - -6.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '880.005' +- start_time_seconds: '880.006' action: type: move robot: honey + from_name: jem_bay7 to_name: jem_bay6 to_pos: - 11.0 - -9.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '890.006' +- start_time_seconds: '890.007' action: type: panorama robot: bumble @@ -88,7 +93,7 @@ - -6.0 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '900.006' +- start_time_seconds: '900.007' action: type: panorama robot: honey @@ -98,17 +103,18 @@ - -9.0 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '1670.007' +- start_time_seconds: '1670.008' action: type: move robot: bumble + from_name: jem_bay3 to_name: jem_bay2 to_pos: - 11.0 - -5.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '1690.008' +- start_time_seconds: '1690.009' action: type: panorama robot: bumble @@ -118,17 +124,18 @@ - -5.0 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '2470.009' +- start_time_seconds: '2470.010' action: type: move robot: bumble + from_name: jem_bay2 to_name: jem_bay1 to_pos: - 11.0 - -4.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '2490.010' +- start_time_seconds: '2490.011' action: type: panorama robot: bumble @@ -138,23 +145,48 @@ - -4.0 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '3270.011' +- start_time_seconds: '3270.012' action: type: stereo robot: bumble fplan: jem_stereo_mapping_bay1_to_bay3.fplan + base_name: jem_bay1 + bound_name: jem_bay4 duration_seconds: '600.000' -- start_time_seconds: '3870.012' +- start_time_seconds: '3870.013' + action: + type: move + robot: bumble + from_name: jem_bay1 + to_name: jem_bay2 + to_pos: + - 11.0 + - -5.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '3870.013' action: type: move robot: honey + from_name: jem_bay6 to_name: jem_bay5 to_pos: - 11.0 - -8.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '3890.013' +- start_time_seconds: '3890.014' + action: + type: move + robot: bumble + from_name: jem_bay2 + to_name: jem_bay3 + to_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '3890.014' action: type: panorama robot: honey @@ -164,29 +196,144 @@ - -8.0 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '4670.014' +- start_time_seconds: '4670.015' action: type: move robot: honey + from_name: jem_bay5 to_name: jem_bay6 to_pos: - 11.0 - -9.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '4690.015' +- start_time_seconds: '4690.016' + action: + type: move + robot: bumble + from_name: jem_bay3 + to_name: jem_bay4 + to_pos: + - 11.0 + - -7.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4690.016' action: type: move robot: honey + from_name: jem_bay6 to_name: jem_bay7 to_pos: - 11.0 - -9.7 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '4710.016' +- start_time_seconds: '4710.017' action: type: dock robot: honey berth: berth2 duration_seconds: '30.000' +- start_time_seconds: '4710.017' + action: + type: move + robot: bumble + from_name: jem_bay4 + to_name: jem_bay3 + to_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4730.018' + action: + type: move + robot: bumble + from_name: jem_bay3 + to_name: jem_bay2 + to_pos: + - 11.0 + - -5.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4740.018' + action: + type: undock + robot: honey + duration_seconds: '30.000' +- start_time_seconds: '4770.019' + action: + type: stereo + robot: honey + fplan: jem_stereo_mapping_bay4_to_bay7.fplan + base_name: jem_bay7 + bound_name: jem_bay4 + duration_seconds: '600.000' +- start_time_seconds: '5370.020' + action: + type: dock + robot: honey + berth: berth2 + duration_seconds: '30.000' +- start_time_seconds: '5370.020' + action: + type: move + robot: bumble + from_name: jem_bay2 + to_name: jem_bay3 + to_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5390.021' + action: + type: move + robot: bumble + from_name: jem_bay3 + to_name: jem_bay4 + to_pos: + - 11.0 + - -7.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5410.022' + action: + type: move + robot: bumble + from_name: jem_bay4 + to_name: jem_bay5 + to_pos: + - 11.0 + - -8.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5430.023' + action: + type: move + robot: bumble + from_name: jem_bay5 + to_name: jem_bay6 + to_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5450.024' + action: + type: move + robot: bumble + from_name: jem_bay6 + to_name: jem_bay7 + to_pos: + - 11.0 + - -9.7 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5470.025' + action: + type: dock + robot: bumble + berth: berth1 + duration_seconds: '30.000' diff --git a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl index 36041f47..aff5b076 100644 --- a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl @@ -92,6 +92,16 @@ ?order - order ?base ?bound - location ) + + ;; completed-let-other-robot-reach: The goal to add if you want one robot to wait for the + ;; other to reach a certain location before pursuing its remaining goals (ones with larger + ;; ?order values). This basically enables a user to provide a specific kind of + ;; between-robots ordering hint to the planner. + (completed-let-other-robot-reach + ?robot - robot + ?order - order + ?loc - location + ) ) (:functions @@ -321,4 +331,35 @@ ) ) + (:durative-action let-other-robot-reach + :parameters ( + ?robot - robot + ?order - order + ?other-loc - location ;; location other robot needs to reach + ?other-robot - robot + ) + :duration (= ?duration 0) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check order + (at start (< (robot-order ?robot) (order-identity ?order))) + + ;; Check parameters make sense + (at start (robots-different ?robot ?other-robot)) + + ;; The main point is to wait until this condition is met + (at start (robot-at ?other-robot ?other-loc)) + ) + :effect + (and + ;; Update order + (at end (assign (robot-order ?robot) (order-identity ?order))) + + ; Mark success + (at end (completed-let-other-robot-reach ?robot ?order ?other-loc)) + ) + ) ) diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl index 7557c669..d099ca79 100644 --- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -21,9 +21,12 @@ (completed-panorama bumble o2 jem_bay2) (completed-panorama bumble o3 jem_bay1) (completed-stereo bumble o4 jem_bay1 jem_bay4) - (completed-panorama honey o0 jem_bay7) - (completed-panorama honey o1 jem_bay6) - (completed-panorama honey o2 jem_bay5) + (robot-at bumble berth1) + (completed-let-other-robot-reach honey o0 jem_bay5) + (completed-panorama honey o1 jem_bay7) + (completed-panorama honey o2 jem_bay6) + (completed-panorama honey o3 jem_bay5) + (completed-stereo honey o4 jem_bay7 jem_bay4) (robot-at honey berth2) ) ) @@ -145,6 +148,7 @@ (location-available jem_bay7) (location-available jem_bay8) (need-stereo bumble o4 jem_bay1 jem_bay4) + (need-stereo honey o4 jem_bay7 jem_bay4) ;; === Static numeric fluents === (= (order-identity o0) 0) @@ -257,15 +261,16 @@ ;; # use the same plan as without this goal and then add some motion actions at the end to achieve this ;; # goal. Instead, it falls back to only undocking one robot at a time, which slows things down by ;; # about 2x. -;; # - {type: robot_at, robot: bumble, location: berth1} +;; - {type: robot_at, robot: bumble, location: berth1} ;; -;; - {type: panorama, robot: honey, order: 0, location: jem_bay7} -;; - {type: panorama, robot: honey, order: 1, location: jem_bay6} -;; - {type: panorama, robot: honey, order: 2, location: jem_bay5} +;; - {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5} +;; - {type: panorama, robot: honey, order: 1, location: jem_bay7} +;; - {type: panorama, robot: honey, order: 2, location: jem_bay6} +;; - {type: panorama, robot: honey, order: 3, location: jem_bay5} ;; ;; # This is another objective we want to include that for some reason causes POPF to fail to generate ;; # a plan (hang indefinitely). No obvious reason why it should cause a problem. -;; # - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7} +;; - {type: stereo, robot: honey, order: 4, trajectory: jem_bay4_to_bay7} ;; ;; - {type: robot_at, robot: honey, location: berth2} ;; diff --git a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py index 3d9e9374..79bc23af 100755 --- a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py @@ -31,12 +31,17 @@ """ import argparse +import collections import pathlib import re import sys -from typing import Any, Dict, Iterable, List +from typing import Any, Dict, Iterable, List, Optional, Tuple, TypeVar +import numpy as np import yaml +from matplotlib import collections as mc +from matplotlib import patches as mp +from matplotlib import pyplot as plt from problem_generator import CWD, DATA_DIR, PDDL_DIR, load_yaml, path_list @@ -45,17 +50,26 @@ # Dynamic config not needed for interpreting the plan ] -ACTION_TYPE_OPTIONS = ("dock", "undock", "move", "panorama", "stereo") - +ACTION_TYPE_OPTIONS = ( + "dock", + "undock", + "move", + "panorama", + "stereo", + "let-other-robot-reach", +) +COLORS = {"bumble": "#4080ffff", "honey": "#c0c080ff"} # Type alias YamlMapping = Dict[str, Any] FloatStr = str # String representation of a floating-point value +T = TypeVar("T") # POPF plan output weirdly mixes its random debugging output with the actual plan actions that it # outputs in a standard format. (Maybe there's some way to suppress the debug info?) Anyway, this # regex works nicely to ignore the debug info and parse the fields of the plan actions we care -# about. It might need fine-tuning if we use other planners. +# about. It might need fine-tuning if we use other planners. Note: The regex also seems to work ok +# with OPTIC, which was derived from POPF. PLAN_ACTION_REGEX = re.compile( r"^(?P\d+(\.\d*)?): (?P\(.*?\))\s+\[(?P\d+(\.\d*)?)\]\s*$" ) @@ -77,7 +91,9 @@ def __repr__(self): return f"{self.start_time_seconds}: {self.action} [{self.duration_seconds}]" -def yaml_action_from_pddl(action: str, static_config: YamlMapping) -> YamlMapping: +def yaml_action_from_pddl( + action: str, static_config: YamlMapping +) -> Optional[YamlMapping]: """ Return a YamlMapping representation of `action`. This is the only place we really need domain-specific logic. @@ -99,11 +115,12 @@ def yaml_action_from_pddl(action: str, static_config: YamlMapping) -> YamlMappin return {"type": "undock", "robot": robot} if action_type == "move": - robot, _from_bay, to_bay, _check_bay = action_args[1:] - # Can discard from_bay, check_bay. Look up coordinates for to_bay. + robot, from_bay, to_bay, _check_bay = action_args[1:] + # Can discard check_bay. Look up coordinates for to_bay. return { "type": "move", "robot": robot, + "from_name": from_bay, "to_name": to_bay, "to_pos": static_config["bays"][to_bay], } @@ -130,8 +147,17 @@ def yaml_action_from_pddl(action: str, static_config: YamlMapping) -> YamlMappin len(traj_matches) == 1 ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" fplan = traj_matches[0]["fplan"] - # Can discard order, base, bound, check1, check2. - return {"type": "stereo", "robot": robot, "fplan": fplan} + # Can discard order check1, check2. + return { + "type": "stereo", + "robot": robot, + "fplan": fplan, + "base_name": base, + "bound_name": bound, + } + + if action_type == "let-other-robot-reach": + return None # Action is a no-op intended only to constrain the planner assert False, "Never reach this point." return {} # Make pylint happy @@ -139,13 +165,16 @@ def yaml_action_from_pddl(action: str, static_config: YamlMapping) -> YamlMappin def yaml_plan_action_from_pddl( plan_action: PlanAction, static_config: YamlMapping -) -> YamlMapping: +) -> Optional[YamlMapping]: """ Return a YamlMapping representation of `plan_action`. """ + action = yaml_action_from_pddl(plan_action.action, static_config) + if action is None: + return None return { "start_time_seconds": plan_action.start_time_seconds, - "action": yaml_action_from_pddl(plan_action.action, static_config), + "action": action, "duration_seconds": plan_action.duration_seconds, } @@ -170,10 +199,26 @@ def parse_plan(plan_path: pathlib.Path) -> List[PlanAction]: float(start_time_seconds) float(duration_seconds) + # One of our PDDL planners being evaluated is OPTIC, which is an anytime + # planner. Its overall output potentially includes multiple plans (with increasing + # quality) output during the course of a single planner run. We want to use only the + # last plan. An easy way to do that is to discard all previous actions if we see a + # new action starting at the overall plan start time (t = 0). + if float(start_time_seconds) == 0 and actions: + print( + "WARNING: Found the start of another plan; will use only the last plan", + file=sys.stderr, + ) + actions = [] + actions.append(PlanAction(start_time_seconds, action, duration_seconds)) return actions +def filter_none(elts: Iterable[Optional[T]]) -> List[T]: + return [x for x in elts if x is not None] + + class NoAliasDumper( yaml.SafeDumper ): # pylint: disable=too-many-ancestors # It only has 1, so (?) @@ -185,10 +230,97 @@ def ignore_aliases(self, data): return True +def save_plan_yaml(output_path: pathlib.Path, yaml_actions: List[YamlMapping]) -> None: + with output_path.open("w", encoding="utf-8") as output_stream: + yaml.dump(yaml_actions, output_stream, Dumper=NoAliasDumper, sort_keys=False) + print(f"Wrote YAML plan to {output_path}", file=sys.stderr) + + +def plot_plan( + plot_path: pathlib.Path, yaml_actions: List[YamlMapping], config: YamlMapping +) -> None: + robot_time_pos = collections.defaultdict(list) + # robot_dock_points = collections.defaultdict(list) + robot_pano_lines = collections.defaultdict(list) + robot_stereo_rects = collections.defaultdict(list) + + for plan_action in yaml_actions: + action = plan_action["action"] + robot = action["robot"] + start_time_seconds = float(plan_action["start_time_seconds"]) + duration_seconds = float(plan_action["duration_seconds"]) + end_time_seconds = start_time_seconds + duration_seconds + if action["type"] == "move": + from_bay = int(action["from_name"][-1]) + to_bay = int(action["to_name"][-1]) + robot_time_pos[robot] += [ + (start_time_seconds, from_bay), + (end_time_seconds, to_bay), + ] + elif action["type"] == "undock": + from_bay = 8 + to_bay = 7 + robot_time_pos[robot] += [ + (start_time_seconds, from_bay), + (end_time_seconds, to_bay), + ] + elif action["type"] == "dock": + from_bay = 7 + to_bay = 8 + robot_time_pos[robot] += [ + (start_time_seconds, from_bay), + (end_time_seconds, to_bay), + ] + elif action["type"] == "panorama": + bay_number = int(action["location_name"][-1]) + robot_pano_lines[robot].append( + ((start_time_seconds, bay_number), (end_time_seconds, bay_number)) + ) + elif action["type"] == "stereo": + base_bay = int(action["base_name"][-1]) + bound_bay = int(action["bound_name"][-1]) + bay1, bay2 = sorted((base_bay, bound_bay)) + padding = 0.5 + robot_stereo_rects[robot].append( + mp.Rectangle( + xy=(start_time_seconds, bay1 - padding), + width=duration_seconds, + height=bay2 - bay1 + 2 * padding, + ) + ) + + fig, ax = plt.subplots() + for robot in config["robots"]: + time_pos = np.array(robot_time_pos[robot]) + plt.plot(time_pos[:, 0], time_pos[:, 1], "o-", label=robot, color=COLORS[robot]) + + if robot_pano_lines[robot]: + ax.add_collection( + mc.LineCollection( + robot_pano_lines[robot], color=COLORS[robot], linewidth=10 + ) + ) + if robot_stereo_rects[robot]: + ax.add_collection( + mc.PatchCollection( + robot_stereo_rects[robot], facecolor=COLORS[robot], alpha=0.25 + ) + ) + + plt.xlabel("Time (s)") + plt.ylabel("Bay number (dock = 8)") + plt.grid("both") + plt.legend() + fig.set_size_inches((24, 4)) + plt.savefig(plot_path) + print(f"Wrote plot to {plot_path}", file=sys.stderr) + + def plan_interpreter( config_paths: Iterable[pathlib.Path], plan_path: pathlib.Path, output_path: pathlib.Path, + plot_path: Optional[pathlib.Path], ) -> None: """ The main function that interprets an entire plan file. @@ -200,12 +332,17 @@ def plan_interpreter( for config_path in config_paths: config.update(load_yaml(config_path)) pddl_actions = parse_plan(plan_path) - yaml_actions = [ - yaml_plan_action_from_pddl(plan_action, config) for plan_action in pddl_actions - ] - with output_path.open("w", encoding="utf-8") as output_stream: - yaml.dump(yaml_actions, output_stream, Dumper=NoAliasDumper, sort_keys=False) - print(f"Wrote to {output_path}", file=sys.stderr) + yaml_actions = filter_none( + [ + yaml_plan_action_from_pddl(plan_action, config) + for plan_action in pddl_actions + ] + ) + + save_plan_yaml(output_path, yaml_actions) + + if plot_path is not None: + plot_plan(plot_path, yaml_actions, config) class CustomFormatter( @@ -237,10 +374,19 @@ def main(): type=pathlib.Path, default=DATA_DIR / "sample_output_plan.yaml", ) + parser.add_argument( + "--plot", + help="Write plot to specified path", + type=lambda arg: None if arg is None else pathlib.Path(arg), + default=None, + ) args = parser.parse_args() plan_interpreter( - config_paths=args.config, plan_path=args.plan, output_path=args.output + config_paths=args.config, + plan_path=args.plan, + output_path=args.output, + plot_path=args.plot, ) diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py index 141f6a36..08432a0a 100755 --- a/astrobee/survey_manager/survey_planner/tools/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -49,7 +49,7 @@ import yaml -GOAL_TYPE_OPTIONS = ("panorama", "stereo", "robot_at") +GOAL_TYPE_OPTIONS = ("panorama", "stereo", "robot_at", "let_other_robot_reach") THIS_DIR = pathlib.Path(__file__).resolve().parent CWD = pathlib.Path.cwd() @@ -105,6 +105,12 @@ def pddl_goal_from_yaml(goal: YamlMapping, config_static: YamlMapping) -> str: location = goal["location"] return f"(robot-at {robot} {location})" + if goal_type == "let_other_robot_reach": + robot = goal["robot"] + location = goal["location"] + order = goal["order"] + return f"(completed-let-other-robot-reach {robot} o{order} {location})" + assert False, "Never reach this point" return {} # Make pylint happy From 3091d4a2cbbc29d403bfa6308e70d47c3c631f1d Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Thu, 21 Dec 2023 14:33:07 -0500 Subject: [PATCH 10/11] Fix pylint and mypy complaints --- .../survey_planner/tools/plan_interpreter.py | 36 +++++++++++-------- .../survey_planner/tools/problem_generator.py | 7 ++-- 2 files changed, 25 insertions(+), 18 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py index 79bc23af..73477859 100755 --- a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py @@ -35,7 +35,7 @@ import pathlib import re import sys -from typing import Any, Dict, Iterable, List, Optional, Tuple, TypeVar +from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple, TypeVar import numpy as np import yaml @@ -43,7 +43,7 @@ from matplotlib import patches as mp from matplotlib import pyplot as plt -from problem_generator import CWD, DATA_DIR, PDDL_DIR, load_yaml, path_list +from problem_generator import DATA_DIR, load_yaml, path_list DEFAULT_CONFIGS = [ DATA_DIR / "jem_survey_static.yaml", @@ -59,11 +59,12 @@ "let-other-robot-reach", ) COLORS = {"bumble": "#4080ffff", "honey": "#c0c080ff"} +ROBOTS = list(COLORS.keys()) # Type alias YamlMapping = Dict[str, Any] FloatStr = str # String representation of a floating-point value -T = TypeVar("T") +T = TypeVar("T") # pylint: disable=invalid-name # POPF plan output weirdly mixes its random debugging output with the actual plan actions that it # outputs in a standard format. (Maybe there's some way to suppress the debug info?) Anyway, this @@ -183,7 +184,7 @@ def parse_plan(plan_path: pathlib.Path) -> List[PlanAction]: """ Return a list of PlanActions read from the PDDL plan at `plan_path`. """ - actions = [] + actions: List[PlanAction] = [] with plan_path.open(encoding="utf-8") as plan_stream: for plan_line in plan_stream: match = PLAN_ACTION_REGEX.search(plan_line) @@ -216,6 +217,7 @@ def parse_plan(plan_path: pathlib.Path) -> List[PlanAction]: def filter_none(elts: Iterable[Optional[T]]) -> List[T]: + "Return `elts` filtered to remove `None` values." return [x for x in elts if x is not None] @@ -231,18 +233,21 @@ def ignore_aliases(self, data): def save_plan_yaml(output_path: pathlib.Path, yaml_actions: List[YamlMapping]) -> None: + "Save plan `yaml_actions` to `output_path` in YAML format." with output_path.open("w", encoding="utf-8") as output_stream: yaml.dump(yaml_actions, output_stream, Dumper=NoAliasDumper, sort_keys=False) print(f"Wrote YAML plan to {output_path}", file=sys.stderr) -def plot_plan( - plot_path: pathlib.Path, yaml_actions: List[YamlMapping], config: YamlMapping -) -> None: - robot_time_pos = collections.defaultdict(list) - # robot_dock_points = collections.defaultdict(list) - robot_pano_lines = collections.defaultdict(list) - robot_stereo_rects = collections.defaultdict(list) +def plot_plan(plot_path: pathlib.Path, yaml_actions: List[YamlMapping]) -> None: + "Save a plot image of plan `yaml_actions` to `plot_path`." + # Type aliases + TimePos = Tuple[float, int] + PanoLine = Tuple[TimePos, TimePos] + + robot_time_pos: Mapping[str, List[TimePos]] = collections.defaultdict(list) + robot_pano_lines: Mapping[str, List[PanoLine]] = collections.defaultdict(list) + robot_stereo_rects: Mapping[str, List[mp.Rectangle]] = collections.defaultdict(list) for plan_action in yaml_actions: action = plan_action["action"] @@ -289,8 +294,8 @@ def plot_plan( ) ) - fig, ax = plt.subplots() - for robot in config["robots"]: + fig, ax = plt.subplots() # pylint: disable=invalid-name + for robot in ROBOTS: time_pos = np.array(robot_time_pos[robot]) plt.plot(time_pos[:, 0], time_pos[:, 1], "o-", label=robot, color=COLORS[robot]) @@ -342,16 +347,17 @@ def plan_interpreter( save_plan_yaml(output_path, yaml_actions) if plot_path is not None: - plot_plan(plot_path, yaml_actions, config) + plot_plan(plot_path, yaml_actions) class CustomFormatter( argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter ): - pass + "Custom formatter for argparse that combines mixins." def main(): + "Parse arguments and invoke plan_interpreter()." parser = argparse.ArgumentParser( description=__doc__, formatter_class=CustomFormatter ) diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py index 08432a0a..72c77327 100755 --- a/astrobee/survey_manager/survey_planner/tools/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -62,7 +62,7 @@ # Type alias YamlMapping = Dict[str, Any] -T = TypeVar("T") +T = TypeVar("T") # pylint: disable=invalid-name # Replace the text "{{ foo }}" in the template with the value of the foo parameter TEMPLATE_SUBST_REGEX = re.compile(r"{{\s*([\w]+)\s*}}") @@ -129,7 +129,7 @@ def pairwise(elts: Iterable[T]) -> Iterable[Tuple[T, T]]: Returns consecutive pairs drawn from `elts`. Back-port itertools.pairwise() to earlier Python versions. """ - a, b = itertools.tee(elts) + a, b = itertools.tee(elts) # pylint: disable=invalid-name next(b, None) return zip(a, b) @@ -139,7 +139,7 @@ def both_ways(elts: Iterable[Tuple[T, T]]) -> Iterable[Tuple[T, T]]: Given `elts` an iterable of 2-tuples, return a new iterable that includes each 2-tuple twice, once in its original order and once reversed. """ - for a, b in elts: + for a, b in elts: # pylint: disable=invalid-name yield a, b yield b, a @@ -148,6 +148,7 @@ def distinct_pairs(elts: Iterable[T]) -> Iterable[Tuple[T, T]]: """ Return distinct pairs of items drawn from `elts`. """ + # pylint: disable=invalid-name for a, b in itertools.product(elts, repeat=2): if a != b: yield a, b From 8a3db0c6bd97c74b17675fdb26e18e3f0be3df15 Mon Sep 17 00:00:00 2001 From: Brian Coltin Date: Tue, 26 Dec 2023 12:15:09 -0800 Subject: [PATCH 11/11] Make predicates on one line. --- .../survey_planner/pddl/domain_survey.pddl | 18 +++--------------- 1 file changed, 3 insertions(+), 15 deletions(-) diff --git a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl index aff5b076..5e197671 100644 --- a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl @@ -72,11 +72,7 @@ ;; completed-panorama: The goal to add if you want the plan to include collecting a ;; panorama. For now, goals specify ?robot and ?order parameters that constrain ;; multi-robot task allocation and task ordering. - (completed-panorama - ?robot - robot - ?order - order - ?location - location - ) + (completed-panorama ?robot - robot ?order - order ?location - location ) ;; completed-stereo: The goal to add if you want the plan to include collecting a stereo ;; survey. For now, goals specify ?robot and ?order parameters that constrain multi-robot @@ -87,21 +83,13 @@ ;; used for collision checking. It's assumed that ?base and ?bound are not adjacent ;; locations. If future stereo surveys violate these assumptions the model will need to be ;; revisited. - (completed-stereo - ?robot - robot - ?order - order - ?base ?bound - location - ) + (completed-stereo ?robot - robot ?order - order ?base ?bound - location ) ;; completed-let-other-robot-reach: The goal to add if you want one robot to wait for the ;; other to reach a certain location before pursuing its remaining goals (ones with larger ;; ?order values). This basically enables a user to provide a specific kind of ;; between-robots ordering hint to the planner. - (completed-let-other-robot-reach - ?robot - robot - ?order - order - ?loc - location - ) + (completed-let-other-robot-reach ?robot - robot ?order - order ?loc - location ) ) (:functions