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 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 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..316a3101 100644 --- a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml @@ -24,27 +24,25 @@ 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: 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} +# 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 db617126..fbfd7e02 100644 --- a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml @@ -40,19 +40,18 @@ bays: 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: - # 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 6a119031..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 domain_survey.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%] -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 +(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 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] +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.585s -user 0m0.553s -sys 0m0.032s +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 d17b7e1c..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 @@ -47,9 +50,8 @@ - 11.0 - -7.0 - 4.8 - run: 1 - duration_seconds: '900.000' -- start_time_seconds: '100.004' + duration_seconds: '780.000' +- start_time_seconds: '100.005' action: type: panorama robot: honey @@ -58,29 +60,30 @@ - 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.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: '1000.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: '1010.006' +- start_time_seconds: '890.007' action: type: panorama robot: bumble @@ -89,9 +92,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.007' action: type: panorama robot: honey @@ -100,19 +102,19 @@ - 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.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: '1930.008' +- start_time_seconds: '1690.009' action: type: panorama robot: bumble @@ -121,19 +123,19 @@ - 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.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: '2850.010' +- start_time_seconds: '2490.011' action: type: panorama robot: bumble @@ -142,26 +144,49 @@ - 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.012' action: type: stereo robot: bumble fplan: jem_stereo_mapping_bay1_to_bay3.fplan - run: 1 + base_name: jem_bay1 + bound_name: jem_bay4 duration_seconds: '600.000' -- start_time_seconds: '4350.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: '4370.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 @@ -170,31 +195,145 @@ - 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.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: '5290.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: '5310.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 e31999fd..5e197671 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,36 +66,30 @@ ;; 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. - (completed-panorama - ?robot - robot - ?order - order - ?location - location - ?run-number - run-number - ) + ;; multi-robot task allocation and task ordering. + (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 - ;; 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. - (completed-stereo - ?robot - robot - ?order - order - ?base ?bound - location - ?run-number - run-number - ) + ;; 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 ) + + ;; 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 @@ -237,7 +230,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 +254,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 +267,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 +283,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,11 +312,42 @@ ;; 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)) ) ) + (: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/jem_survey_template.pddl b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl index 313ca28f..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,10 +3,7 @@ (:domain survey-manager) (:metric minimize (total-time)) (:objects - {{ bays }} {{ berths }} - location - {{ robots }} - robot - {{ orders }} - order - run1 run2 run3 run4 run5 - run-number + {{ objects }} ) (:goal @@ -17,41 +14,21 @@ (:init ;; === Static predicates === - {{ move_connected_predicates }} - - {{ location_real_predicates }} - - {{ dock_connected_predicates }} - - {{ robots_different_predicates }} - - {{ locations_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 ;; 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..d099ca79 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) @@ -9,20 +11,22 @@ (: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 - run1 run2 run3 run4 run5 - run-number + o0 o1 o2 o3 o4 - order ) (: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) + (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) ) ) @@ -45,7 +49,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) @@ -53,13 +56,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) @@ -136,10 +136,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) @@ -149,10 +147,8 @@ (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 run1) + (need-stereo bumble o4 jem_bay1 jem_bay4) + (need-stereo honey o4 jem_bay7 jem_bay4) ;; === Static numeric fluents === (= (order-identity o0) 0) @@ -160,11 +156,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) @@ -174,65 +165,7 @@ ;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. -;; BEGIN CONFIG_DYNAMIC -;; # 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_DYNAMIC - -;; BEGIN CONFIG_STATIC +;; BEGIN CONFIG ;; # Copyright (c) 2023, United States Government, as represented by the ;; # Administrator of the National Aeronautics and Space Administration. ;; # @@ -250,7 +183,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. -;; +;; ;; # 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 +191,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 +204,11 @@ ;; 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 +224,59 @@ ;; 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} +;; - {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 +;; # 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: 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: 4, trajectory: jem_bay4_to_bay7} +;; +;; - {type: robot_at, robot: honey, location: berth2} +;; +;; init: +;; bumble: +;; location: berth1 +;; honey: +;; location: berth2 +;; END CONFIG 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/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py index ce98e096..73477859 100755 --- a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py +++ b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py @@ -31,37 +31,51 @@ """ import argparse +import collections import pathlib import re import sys -from typing import Any, Dict, List +from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple, TypeVar +import numpy as np import yaml - -ACTION_TYPE_OPTIONS = ("dock", "undock", "move", "panorama", "stereo") - +from matplotlib import collections as mc +from matplotlib import patches as mp +from matplotlib import pyplot as plt + +from problem_generator import DATA_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", + "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") # 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 # 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*$" ) -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. @@ -78,7 +92,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. @@ -100,30 +116,28 @@ 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], } 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 @@ -134,8 +148,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, "run": run_number} + # 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 @@ -143,13 +166,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, } @@ -158,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) @@ -174,10 +200,27 @@ 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 `elts` filtered to remove `None` values." + 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 (?) @@ -189,8 +232,100 @@ def ignore_aliases(self, data): return True +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]) -> 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"] + 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() # 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]) + + 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_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, + plot_path: Optional[pathlib.Path], ) -> None: """ The main function that interprets an entire plan file. @@ -198,52 +333,66 @@ 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 - ] - 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) 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 ) 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", + ) + 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_static_path=args.config_static, + 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 7b5b2b31..72c77327 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,19 +36,33 @@ """ import argparse +import collections +import io import itertools +import os import pathlib import re +import shlex import sys -from typing import Any, Dict, Iterable, Sequence, T, Tuple +from abc import ABC, abstractmethod +from typing import Any, Dict, Iterable, List, Mapping, Sequence, Tuple, TypeVar 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() +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", +] # Type alias YamlMapping = Dict[str, Any] +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*}}") @@ -68,24 +89,28 @@ 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"] 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 @@ -104,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) @@ -114,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 @@ -123,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 @@ -136,11 +162,11 @@ 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( - "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,55 +175,205 @@ 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()]) + + +class ProblemWriter(ABC): + "Abstract class for writing a problem intance." + + @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.)" + + @abstractmethod + def get_extension(self) -> str: + "Return standard extension to use for output file." + + @abstractmethod + def declare_instance(self, instance_name: str, pddl_type: str) -> None: + "Declare `instance_name` as an instance of `pddl_type`." + + 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.declare_instance(instance_name, pddl_type) + + @abstractmethod + def declare_predicate(self, predicate: str, section: str) -> None: + "Declare `predicate` as a PDDL predicate in `section`." + + def declare_predicates(self, predicates: Iterable[str], section: str) -> None: + "Declare `predicates` as PDDL predicates in `section`." + for predicate in predicates: + self.declare_predicate(predicate, section) + + @abstractmethod + def declare_goals(self, goals: Iterable[str]) -> None: + "Declare `goals` as PDDL goals. (Can only be called once, with all goals.)" + + @abstractmethod + def declare_fluent(self, fluent: str, section: str) -> None: + "Declare `fluent` as a PDDL fluent in `section`." + + def declare_fluents(self, fluents: Iterable[str], section: str) -> None: + "Declare `fluents` as PDDL fluents in `section`." + for fluent in fluents: + 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( problem_template_path: pathlib.Path, - config_dynamic_path: pathlib.Path, - config_static_path: pathlib.Path, - output_path: pathlib.Path, + config_paths: Iterable[pathlib.Path], + output_path_template: pathlib.Path, + command: str, + terminal: bool, ) -> 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) - params = {} - - params["header"] = ( - ";; Auto-generated by problem_generator.py. Do not edit!\n" - f";; Problem template: {problem_template_path}\n" - f";; Dynamic config: {config_dynamic_path}\n" - f";; Static config: {config_static_path}\n" - ) - - bays = list(config_static["bays"].keys()) - bogus_bays = config_static["bogus_bays"] + config = {} + for config_path in config_paths: + config.update(load_yaml(config_path)) + + if terminal: + writer: ProblemWriter = TerminalWriter() + else: + 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) - params["bays"] = " ".join(all_bays) + writer.declare_instances(all_bays, "location") - berths = config_static["berths"] - params["berths"] = " ".join(berths) + berths = config["berths"] + writer.declare_instances(berths, "location") - robots = config_static["robots"] - params["robots"] = " ".join(robots) + robots = config["robots"] + writer.declare_instances(robots, "robot") - num_orders = config_static["num_orders"] - params["orders"] = " ".join([f"o{i}" for i in range(num_orders)]) + 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)] + writer.declare_instances(orders, "order") - yaml_goals = config_dynamic["goals"] - pddl_goals = [pddl_goal_from_yaml(goal, config_static) for goal in yaml_goals] - params["goals"] = indent_lines(pddl_goals, 12) + yaml_goals = config["goals"] + pddl_goals = [pddl_goal_from_yaml(goal, config) for goal in yaml_goals] + writer.declare_goals(pddl_goals) 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) + writer.declare_predicates(move_connected_lines, "static_predicates") location_real_lines = [f"(location-real {bay})" for bay in bays] - 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 = [ @@ -205,28 +381,26 @@ 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) + writer.declare_predicates(dock_connected_lines, "static_predicates") 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) + writer.declare_predicates(robots_different_lines, "static_predicates") - 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 - ) + writer.declare_predicates(locs_different_lines, "static_predicates") robot_available_lines = [f"(robot-available {robot})" for robot in robots] - params["robot_available_predicates"] = indent_lines(robot_available_lines, 8) + writer.declare_predicates(robot_available_lines, "dynamic_predicates") - init = config_dynamic["init"] + 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) + writer.declare_predicates(robot_at_lines, "dynamic_predicates") all_locations = all_bays + berths occupied_locations = [init[robot]["location"] for robot in robots] @@ -234,72 +408,78 @@ 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) + 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 ] - 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)] - 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] - params["robot_order_fluents"] = indent_lines(robot_order_lines, 8) + writer.declare_fluents(robot_order_lines, "dynamic_fluents") - params["config_dynamic"] = comment_for_pddl(config_dynamic_path.read_text()) - params["config_static"] = comment_for_pddl(config_static_path.read_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) + 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( - "--problem-template", - help="Path to input PDDL problem template", - type=pathlib.Path, - default="jem_survey_template.pddl", + "-t", + "--terminal", + help="Format output for PlanSys2 terminal instead of PDDL", + action="store_true", + default=False, ) parser.add_argument( - "--config-dynamic", - help="Path to input dynamic problem config YAML (goals, initial state)", + "--problem-template", + help="Path to input PDDL problem template (unused if -t specified)", 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", + help="Path for output PDDL problem ({ext} filled with extension)", type=pathlib.Path, - default="problem_jem_survey.pddl", + default=PDDL_DIR / "problem_jem_survey{ext}", ) args = parser.parse_args() problem_generator( problem_template_path=args.problem_template, - config_dynamic_path=args.config_dynamic, - config_static_path=args.config_static, - output_path=args.output, + config_paths=args.config, + output_path_template=args.output, + command=shlex.join(sys.argv), + terminal=args.terminal, )