Multi robot plan repair

omniplanner/src/dsg_pddl/domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore_Constraints.pddl

@@ -0,0 +1,104 @@
+(define (domain region-object-rearrangement-domain-multirobot-fd-constraints)
+    (:requirements :derived-predicates :typing :adl)
+    (:types
+        region point-of-interest - object
+        place dsg_object - point-of-interest
+        robot - object
+    )
+
+    (:predicates
+        (at-poi ?r - robot ?p - point-of-interest)
+        (connected ?s - point-of-interest ?t - point-of-interest)
+        (suspicious ?o - dsg_object)
+        (at-object ?r - robot ?o - dsg_object)
+        (at-place ?r - robot ?p - place)
+        (in-region ?r - robot ?reg - region)
+
+        (holding ?r - robot ?o - dsg_object)
+        (hand-full ?r - robot)
+        (object-in-place ?o - dsg_object ?p - place)
+        (place-in-region ?p - place ?reg - region)
+
+        (visited-poi ?p - point-of-interest)
+        (visited-place ?p - place)
+        (visited-object ?o - dsg_object)
+        (visited-region ?reg - region)
+
+        (safe ?o - dsg_object)
+        (explored-region ?reg - region)
+
+        (forbidden-poi ?p - point-of-interest)
+        (forbidden-edge ?s - point-of-interest ?t - point-of-interest)
+    )
+
+    (:functions
+        (distance ?s - point-of-interest ?t - point-of-interest)
+        (total-cost)
+    )
+
+    (:derived (at-object ?r - robot ?o - dsg_object)
+        (at-poi ?r ?o))
+
+    (:derived (at-place ?r - robot ?p - place)
+        (at-poi ?r ?p))
+
+    (:derived (visited-place ?p - place)
+        (visited-poi ?p))
+
+    (:derived (visited-object ?o - dsg_object)
+        (visited-poi ?o))
+
+    (:derived (safe ?o - dsg_object)
+        (not (suspicious ?o)))
+
+    (:derived (in-region ?r - robot ?reg - region)
+        (exists (?p - place) (and (at-place ?r ?p) (place-in-region ?p ?reg))))
+
+    (:derived (visited-region ?reg - region)
+        (exists (?p - place) (and (visited-place ?p) (place-in-region ?p ?reg))))
+
+    (:derived (explored-region ?reg - region)
+        (forall (?p - place)
+            (or (not (place-in-region ?p ?reg))
+                (visited-place ?p))))
+
+    (:action goto-poi
+        :parameters (?r - robot ?s - point-of-interest ?t - point-of-interest)
+        :precondition (and (at-poi ?r ?s)
+                           (or (connected ?s ?t) (connected ?t ?s))
+                           (not (forbidden-poi ?t))
+                           (not (forbidden-edge ?s ?t))
+                           (not (forbidden-edge ?t ?s)))
+        :effect (and (not (at-poi ?r ?s))
+                     (at-poi ?r ?t)
+                     (visited-poi ?t)
+                     (increase (total-cost) (distance ?s ?t))
+        )
+    )
+
+    (:action inspect
+     :parameters (?r - robot ?o - dsg_object)
+     :precondition (at-object ?r ?o)
+     :effect (and (not (suspicious ?o))
+                  (increase (total-cost) 1)
+            )
+     )
+
+    (:action pick-object
+     :parameters (?r - robot ?o - dsg_object ?p - place)
+     :precondition (and (not (hand-full ?r))
+                        (safe ?o)
+                        (at-object ?r ?o)
+                        (object-in-place ?o ?p))
+     :effect (and (holding ?r ?o)
+                  (hand-full ?r)
+                  (not (object-in-place ?o ?p))))
+
+    (:action place-object
+     :parameters (?r - robot ?o - dsg_object ?p - place)
+     :precondition (and (holding ?r ?o) (at-place ?r ?p))
+     :effect (and (not (holding ?r ?o))
+                  (not (hand-full ?r))
+                  (object-in-place ?o ?p)))
+
+)

omniplanner/src/dsg_pddl/dsg_pddl_grounding.py

@@ -480,6 +480,7 @@ def ground_problem(
     robot_states: dict,
     goal: PddlGoal,
     feedback: Any = None,
+    constraints: list = [],
 ) -> RobotWrapper[GroundedPddlProblem]:
     logger.info(f"Grounding PDDL Problem {domain.domain_name}")
 

omniplanner/src/dsg_pddl/dsg_pddl_grounding_multirobot.py

@@ -64,7 +64,7 @@ def generate_dense_region_symbol_connectivity_multirobot(G, symbols, robot_state
         10,
         layer_planner,
     )
-    start_connection_threshold = 50
+    start_connection_threshold = 20
     for robot_id in robot_states.keys():
         start_symbol_key = f"pstart{robot_id}"
         if start_symbol_key in symbol_lookup:
@@ -146,8 +146,10 @@ def generate_multirobot_region_pddl(
     G: spark_dsg.DynamicSceneGraph,
     raw_pddl_goal_string: str,
     robot_states: np.ndarray,
+    constraints: List[tuple] = None,
+    domain_name: str = "region-object-rearrangement-domain-multirobot-fd",
 ) -> Tuple[str, List[PddlSymbol]]:
-    """Generate a multi-robot PDDL problem for domain region-object-rearrangement-domain-multirobot-fd."""
+    """Generate a multi-robot PDDL problem for the given multi-robot domain."""
     # Collect all places/objects/regions and positions
     symbols = extract_all_symbols(G)
     normalize_symbols(symbols)
@@ -194,12 +196,17 @@ def generate_multirobot_region_pddl(
     object_symbols = [s for s in symbols_of_interest if s.layer == "object"]
     init_facts_tuples += [("suspicious", o.symbol) for o in object_symbols]
 
+    # Inject runtime constraints (forbidden-poi, forbidden-edge, etc.)
+    if constraints:
+        init_facts_tuples += list(constraints)
+        logger.info(f"Injected {len(constraints)} constraint facts into PDDL init")
+
     # Build objects dict using shared generator and adding robots
     pddl_objects = generate_objects(symbols_of_interest)
     pddl_objects["robot"] = robot_ids
     problem = PddlProblem(
         name="multi-robot-problem",
-        domain="region-object-rearrangement-domain-multirobot-fd",
+        domain=domain_name,
         objects=pddl_objects,
         initial_facts=tuple(init_facts_tuples),
         goal=goal_pddl,
@@ -233,19 +240,22 @@ def ground_problem(
     robot_states: dict,
     goal: PddlGoal,
     feedback: Any = None,
+    constraints: list = [],
 ) -> MultiRobotWrapper[GroundedPddlProblem]:
     logger.warning(f"Grounding PDDL Problem {domain.domain_name}")
 
     pddl_compliant_robot_states = {k.lower(): v for k, v in robot_states.items()}
     match domain.domain_name:
-        # case "goto-object-domain-multirobot-fd":
-        #     pddl_problem, symbols = generate_multirobot_inspection_pddl(
-        #         dsg, goal.pddl_goal, robot_states
-        #     )
-
-        case "region-object-rearrangement-domain-multirobot-fd":
+        case (
+            "region-object-rearrangement-domain-multirobot-fd"
+            | "region-object-rearrangement-domain-multirobot-fd-constraints"
+        ):
             pddl_problem, symbols = generate_multirobot_region_pddl(
-                dsg, goal.pddl_goal, pddl_compliant_robot_states
+                dsg,
+                goal.pddl_goal,
+                pddl_compliant_robot_states,
+                constraints,
+                domain_name=domain.domain_name,
             )
             # logger.warning(f"!!!!!!!!!!!!!!pddl_problem: {pddl_problem}")
         case _:

omniplanner/src/dsg_pddl/dsg_pddl_planning.py

@@ -1,4 +1,7 @@
 import logging
+import os
+import subprocess
+import tempfile
 from dataclasses import dataclass
 from typing import Any, Dict, List
 
@@ -7,7 +10,7 @@
 from plum import dispatch
 
 from dsg_pddl.dsg_pddl_grounding import GroundedPddlProblem, PddlDomain, PddlSymbol
-from dsg_pddl.pddl_planning import solve_pddl
+from dsg_pddl.pddl_utils import lisp_string_to_ast
 from omniplanner.tsp import LayerPlanner
 
 logger = logging.getLogger(__name__)
@@ -21,6 +24,90 @@ class PddlPlan:
     symbols: Dict[str, PddlSymbol]
 
 
+DEFAULT_FD_SEARCH = (
+    "let(hff, ff(), let(hcea, cea(), lazy_greedy([hff, hcea], preferred=[hff, hcea])))"
+)
+
+
+def solve_pddl(problem: GroundedPddlProblem):
+    """Use fast-downward to solve the given pddl problem.
+
+    The Fast Downward search can be overridden via environment variables:
+        ADT4_FD_ALIAS   — use a Fast Downward alias (e.g. "seq-sat-lama-2011").
+                          Takes precedence over ADT4_FD_SEARCH when set.
+        ADT4_FD_SEARCH  — raw value passed after `--search`.
+    If neither is set, the historical lazy-greedy(ff+cea) search is used.
+    """
+    with tempfile.TemporaryDirectory() as tmpdirname:
+        problem_fn = os.path.join(tmpdirname, "problem.pddl")
+        domain_fn = os.path.join(tmpdirname, "domain.pddl")
+        plan_fn = os.path.join(tmpdirname, "plan.txt")
+
+        with open(problem_fn, "w") as fo:
+            fo.write(problem.problem_str)
+
+        with open(domain_fn, "w") as fo:
+            fo.write(problem.domain.to_string())
+
+        alias = os.getenv("ADT4_FD_ALIAS", "").strip()
+        search = os.getenv("ADT4_FD_SEARCH", "").strip()
+        time_limit = os.getenv("ADT4_FD_OVERALL_TIME_LIMIT", "").strip()
+
+        command = ["fast-downward"]
+        command += ["--plan-file", plan_fn]
+        if time_limit:
+            command += ["--overall-time-limit", time_limit]
+        if alias:
+            command += ["--alias", alias]
+            command += [domain_fn, problem_fn]
+        else:
+            command += [domain_fn, problem_fn]
+            command += ["--search", search or DEFAULT_FD_SEARCH]
+
+        logger.warning(f"Calling: {command}")
+        return_code = subprocess.run(command)
+        logger.warning(f"Return code: {return_code}")
+
+        # Anytime aliases (e.g. seq-sat-lama-2011) write plan.txt.1, plan.txt.2,
+        # ... and do NOT write plan.txt. Pick the highest-numbered (best) one.
+        import glob
+
+        numbered = sorted(
+            glob.glob(plan_fn + ".*"),
+            key=lambda p: int(p.rsplit(".", 1)[-1])
+            if p.rsplit(".", 1)[-1].isdigit()
+            else -1,
+        )
+        if numbered and numbered[-1].rsplit(".", 1)[-1].isdigit():
+            best_plan = numbered[-1]
+        elif os.path.exists(plan_fn):
+            best_plan = plan_fn
+        else:
+            best_plan = None
+
+        if best_plan is not None:
+            logger.warning(f"Reading plan from {best_plan}")
+            with open(best_plan, "r") as fo:
+                lines = fo.readlines()
+        else:
+            output_dir = os.getenv("ADT4_OUTPUT_DIR", "")
+            debug_fn = os.path.join(output_dir, "pddl_problem_debugging.pddl")
+            logger.warning(
+                f"Planning failed. Please see {debug_fn} for the failed problem file."
+            )
+            with open(debug_fn, "w") as fo:
+                fo.write(problem.problem_str)
+            raise Exception(
+                f"Planning failed, please see {debug_fn} for failed problem file."
+            )
+
+    plan = [lisp_string_to_ast(line) for line in lines[:-1]]
+    plan = [
+        ("goto-poi",) + p[1:] if p and p[0] == "goto-poi-revisit" else p for p in plan
+    ]
+    return plan
+
+
 def drop_index(t, k):
     return t[:k] + t[k + 1 :]
 
@@ -76,7 +163,7 @@ def parameterize_place_object_multirobot(layer_planner, symbols, action, last_po
 def make_plan(grounded_problem: GroundedPddlProblem, map_context: Any) -> PddlPlan:
     plan = solve_pddl(grounded_problem)
 
-    logger.warning(f"Made plan {plan}")
+    # logger.warning(f"Made plan {plan}")
 
     parameterized_plan = []
 

omniplanner/src/omniplanner/goto_points.py

@@ -49,6 +49,7 @@ def ground_problem(
     robot_states: dict,
     goal: GotoPointsGoal,
     feedback: Any = None,
+    constraints: list = [],
 ) -> RobotWrapper[GroundedGotoPointsProblem]:
     start = robot_states[goal.robot_id]
 
@@ -78,6 +79,7 @@ def ground_problem(
     start: np.ndarray,
     goal: list,
     feedback: Any = None,
+    constraints: list = [],
 ) -> GroundedGotoPointsProblem:
     point_sequence = map_context[goal]
     return GroundedGotoPointsProblem(start, point_sequence)

omniplanner/src/omniplanner/language_planner.py

@@ -29,6 +29,7 @@ def ground_problem(
     robot_states: dict,
     goal: LanguageGoal,
     feedback: Any = None,
+    constraints: list = [],
 ):
     if domain.domain_type == "goto_points":
         language_grounded_goal = GotoPointsGoal(

omniplanner/src/omniplanner/omniplanner.py

@@ -43,6 +43,7 @@ class PlanRequest:
     domain: PlanningDomain
     goal: PlanningGoal
     robot_states: dict
+    constraints: list = field(default_factory=list)  # PDDL constraint facts
 
 
 @dataclass
@@ -248,6 +249,7 @@ def ground_problem(
     intial_state: Any,
     goal: PlanningGoal,
     feedback: Any = None,
+    constraints: list = [],
 ) -> GroundedProblem:
     raise DispatchException(ground_problem, domain, map_context, goal, feedback)
 
@@ -272,6 +274,7 @@ def full_planning_pipeline(plan_request: PlanRequest, map_context: Any, feedback
         plan_request.robot_states,
         plan_request.goal,
         feedback,
+        plan_request.constraints,
     )
     logger.debug("Grounded Problem")
 

omniplanner/src/omniplanner/tsp.py

@@ -150,7 +150,12 @@ class TspGoal:
 @overload
 @dispatch
 def ground_problem(
-    domain: TspDomain, dsg: Any, robot_states: dict, goal: TspGoal, feedback: Any = None
+    domain: TspDomain,
+    dsg: Any,
+    robot_states: dict,
+    goal: TspGoal,
+    feedback: Any = None,
+    constraints: list = [],
 ) -> RobotWrapper[GroundedTspProblem]:
     logger.info("Grounding TSP Problem")
 

omniplanner_msgs/msg/PddlGoalMsg.msg

@@ -1,2 +1,3 @@
 string robot_id
 string pddl_goal
+string constraints  # JSON list of constraint facts, e.g. '[["forbidden-poi","p123"]]'. Empty = no change.

omniplanner_ros/setup.py

@@ -43,6 +43,10 @@ def get_share_info(top_level, pattern, dest=None):
     license="TODO: License declaration",
     tests_require=["pytest"],
     entry_points={
-        "console_scripts": ["omniplanner_node = omniplanner_ros.omniplanner_node:main"],
+        "console_scripts": [
+            "omniplanner_node = omniplanner_ros.omniplanner_node:main",
+            "omniplanner_repair_node = omniplanner_ros.omniplanner_repair_node:main",
+            "goal_manager_node = omniplanner_ros.goal_manager_node:main",
+        ],
     },
 )