Merge pull request #112 from trey0/problem_gen_fixes

Fixes for problem_generator.py

.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

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

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:

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

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*<special>
+- 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