working, but slow. maybe duplicate constraint sets in the outer tree?

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/BaseClasses.py

@@ -20,7 +20,7 @@ class SingleAgentPlanner(ABC):
 
     @staticmethod
     @abstractmethod
-    def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath:
+    def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: bool = False) -> NodePath:
         pass
 
 @dataclass

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -7,15 +7,17 @@
 import numpy as np
 from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
     Grid,
-    Interval,
     ObstacleArrangement,
     Position,
 )
+from copy import deepcopy
 from PathPlanning.TimeBasedPathPlanning.BaseClasses import MultiAgentPlanner, StartAndGoal
 from PathPlanning.TimeBasedPathPlanning.Node import NodePath
 from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
 from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner
+from PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar import SpaceTimeAStar
 from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths
+from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId, AppliedConstraint, ConstraintTree, ConstraintTreeNode, ForkingConstraint
 import time
 
 class ConflictBasedSearch(MultiAgentPlanner):
@@ -29,59 +31,90 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
         """
         print(f"Using single-agent planner: {single_agent_planner_class}")
 
+        initial_solution: dict[AgentId, NodePath] = {}
+
         # Reserve initial positions
-        for start_and_goal in start_and_goals:
-            grid.reserve_position(start_and_goal.start, start_and_goal.index, Interval(0, 10))
+        for agent_idx, start_and_goal in enumerate(start_and_goals):
+            # grid.reserve_position(start_and_goal.start, start_and_goal.index, Interval(0, 10))
+            path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, agent_idx, verbose)
+            initial_solution[AgentId(agent_idx)] = path
+
+        constraint_tree = ConstraintTree(initial_solution)
+
+        while constraint_tree.nodes_to_expand:
+            constraint_tree_node = constraint_tree.get_next_node_to_expand()
+            ancestor_constraints = constraint_tree.get_ancestor_constraints(constraint_tree_node.parent_idx)
+            print(f"Expanded node: {constraint_tree_node.constraint} with parent: {constraint_tree_node.parent_idx}")
+            print(f"\tAncestor constraints: {ancestor_constraints}")
+
+            if verbose:
+                print(f"Expanding node with constraint {constraint_tree_node.constraint} and parent {constraint_tree_node.parent_idx} ")
+
+            if constraint_tree_node is None:
+                raise RuntimeError("No more nodes to expand in the constraint tree.")
+            if not constraint_tree_node.constraint:
+                # This means we found a solution!
+                return (start_and_goals, [constraint_tree_node.paths[AgentId(i)] for i in range(len(start_and_goals))])
+
+            if not isinstance(constraint_tree_node.constraint, ForkingConstraint):
+                raise ValueError(f"Expected a ForkingConstraint, but got: {constraint_tree_node.constraint}")
+
+            # TODO: contents of this loop should probably be in a helper?
+            for constrained_agent in constraint_tree_node.constraint.constrained_agents:
+                paths: dict[AgentId, NodePath] = {}
 
-        # Plan in descending order of distance from start to goal
-        # TODO: dont bother doing this
-        start_and_goals = sorted(start_and_goals,
-                    key=lambda item: item.distance_start_to_goal(),
-                    reverse=True)
+                if verbose:
+                    print(f"\nOuter loop step for agent {constrained_agent}")
 
-        # first, plan optimally for each agent
-        # now in a loop:
-        #   * 
+                applied_constraint = AppliedConstraint(constraint_tree_node.constraint.constraint, constrained_agent)
+                all_constraints = deepcopy(ancestor_constraints)
+                all_constraints.append(applied_constraint)
 
-        # paths = []
-        # for start_and_goal in start_and_goals:
-        #     if verbose:
-        #         print(f"\nPlanning for agent:  {start_and_goal}" )
+                if verbose:
+                    print(f"\tall constraints: {all_constraints}")
 
-        #     grid.clear_initial_reservation(start_and_goal.start, start_and_goal.index)
-        #     path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, verbose)
+                grid.clear_constraint_points()
+                grid.apply_constraint_points(all_constraints)
 
-        #     if path is None:
-        #         print(f"Failed to find path for {start_and_goal}")
-        #         return []
+                for agent_idx, start_and_goal in enumerate(start_and_goals):
+                    path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, agent_idx, verbose)
+                    if path is None:
+                        raise RuntimeError(f"Failed to find path for {start_and_goal}")
+                    paths[AgentId(start_and_goal.index)] = path
 
-        #     agent_index = start_and_goal.index
-        #     grid.reserve_path(path, agent_index)
-        #     paths.append(path)
+                applied_constraint_parent = deepcopy(constraint_tree_node) #TODO: not sure if deepcopy is actually needed
+                applied_constraint_parent.constraint = applied_constraint
+                parent_idx = constraint_tree.add_expanded_node(applied_constraint_parent)
 
-        return (start_and_goals, paths)
+                new_constraint_tree_node = ConstraintTreeNode(paths, parent_idx)
+                if new_constraint_tree_node.constraint is None:
+                    # This means we found a solution!
+                    return (start_and_goals, [paths[AgentId(i)] for i in range(len(start_and_goals))])
 
-    def cbs():
+                if verbose:
+                    print(f"Adding new constraint tree node with constraint: {new_constraint_tree_node.constraint}")
+                constraint_tree.add_node_to_tree(new_constraint_tree_node)
 
 verbose = False
 show_animation = True
 def main():
     grid_side_length = 21
 
-    start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
+    # start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
+    start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(5)]
     obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)]
 
     grid = Grid(
         np.array([grid_side_length, grid_side_length]),
         num_obstacles=250,
         obstacle_avoid_points=obstacle_avoid_points,
-        # obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
-        obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+        obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
+        # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
         # obstacle_arrangement=ObstacleArrangement.RANDOM,
     )
 
     start_time = time.time()
-    start_and_goals, paths = ConflictBasedSearch.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose)
+    start_and_goals, paths = ConflictBasedSearch.plan(grid, start_and_goals, SpaceTimeAStar, verbose)
 
     runtime = time.time() - start_time
     print(f"\nPlanning took: {runtime:.5f} seconds")

PathPlanning/TimeBasedPathPlanning/ConstraintTree.py

@@ -2,68 +2,80 @@
 from typing import Optional, TypeAlias
 import heapq
 
-from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position, PositionAtTime
 
 AgentId: TypeAlias = int
 
-@dataclass
+@dataclass(frozen=True)
 class Constraint:
     position: Position
     time: int
 
 @dataclass
-class PathConstraint:
+class ForkingConstraint:
+    constraint: Constraint
+    constrained_agents: tuple[AgentId, AgentId]
+
+@dataclass(frozen=True)
+class AppliedConstraint:
     constraint: Constraint
-    shorter_path_agent: AgentId
-    longer_path_agent: AgentId
+    constrained_agent: AgentId
 
 @dataclass
 class ConstraintTreeNode:
     parent_idx = int
-    constraint: tuple[AgentId, Constraint]
+    constraint: Optional[ForkingConstraint | AppliedConstraint]
 
     paths: dict[AgentId, NodePath]
     cost: int
 
+    def __init__(self, paths: dict[AgentId, NodePath], parent_idx: int):
+        self.paths = paths
+        self.cost = sum(path.goal_reached_time() for path in paths.values())
+        self.parent_idx = parent_idx
+        self.constraint = self.get_constraint_point()
+
     def __lt__(self, other) -> bool:
-        # TODO - this feels jank?
-        return self.cost + self.constrained_path_cost() < other.cost + other.constrained_path_cost()
+        return self.cost < other.cost
+
+    def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]:
+        if verbose:
+            print(f"\tpath for {agent_id}: {path}\n")
 
-    def get_constraint_point(self) -> Optional[PathConstraint]:
-        final_t = max(path.goal_reached_time() for path in self.paths)
-        positions_at_time: dict[Position, AgentId] = {}
+        final_t = max(path.goal_reached_time() for path in self.paths.values())
+        positions_at_time: dict[PositionAtTime, AgentId] = {}
         for t in range(final_t + 1):
             # TODO: need to be REALLY careful that these agent ids are consitent
             for agent_id, path in self.paths.items():
                 position = path.get_position(t)
                 if position is None:
                     continue
-                if position in positions_at_time:
-                    conflicting_agent_id = positions_at_time[position]
-                    this_agent_shorter = self.paths[agent_id].goal_reached_time() < self.paths[conflicting_agent_id].goal_reached_time()
-
-                    return PathConstraint(
+                # print(f"reserving pos/t for {agent_id}: {position} @ {t}")
+                position_at_time = PositionAtTime(position, t)
+                if position_at_time in positions_at_time:
+                    conflicting_agent_id = positions_at_time[position_at_time]
+
+                    if verbose:
+                        print(f"found constraint: {position_at_time} for agents {agent_id} & {conflicting_agent_id}")
+                    return ForkingConstraint(
                         constraint=Constraint(position=position, time=t),
-                        shorter_path_agent= agent_id if this_agent_shorter else conflicting_agent_id,
-                        longer_path_agent= conflicting_agent_id if this_agent_shorter else agent_id
+                        constrained_agents=(AgentId(agent_id), AgentId(conflicting_agent_id))
                     )
+                else:
+                    positions_at_time[position_at_time] = AgentId(agent_id)
         return None
 
-    def constrained_path_cost(self) -> int:
-        constrained_path = self.paths[self.constraint[0]]
-        return constrained_path.goal_reached_time()
 
 class ConstraintTree:
     # Child nodes have been created (Maps node_index to ConstraintTreeNode)
     expanded_nodes: dict[int, ConstraintTreeNode]
     # Need to solve and generate children
-    nodes_to_expand: heapq[ConstraintTreeNode]
-
-    solution: Optional[ConstraintTreeNode] = None
+    nodes_to_expand: heapq #[ConstraintTreeNode]
 
     def __init__(self, initial_solution: dict[AgentId, NodePath]):
-        initial_cost = sum(path.goal_reached_time() for path in initial_solution.values())
-        heapq.heappush(self.nodes_to_expand, ConstraintTreeNode(constraints={}, paths=initial_solution, cost=initial_cost, parent_idx=-1))
+        self.nodes_to_expand = []
+        self.expanded_nodes = {}
+        heapq.heappush(self.nodes_to_expand, ConstraintTreeNode(initial_solution, -1))
 
     def get_next_node_to_expand(self) -> Optional[ConstraintTreeNode]:
         if not self.nodes_to_expand:
@@ -74,33 +86,26 @@ def add_node_to_tree(self, node: ConstraintTreeNode) -> bool:
         """
         Add a node to the tree and generate children if needed. Returns true if the node is a solution, false otherwise.
         """
-        node_index = len(self.expanded_nodes)
-        self.expanded_nodes[node_index] = node
-        constraint_point = node.get_constraint_point()
-        if constraint_point is None:
-            # Don't need to add any constraints, this is a solution!
-            self.solution = node
-            return
-        
-        child_node1 = node
-        child_node1.constraint = (constraint_point.shorter_path_agent, constraint_point.constraint)
-        child_node1.parent_idx = node_index
-
-        child_node2 = node
-        child_node2.constraint = (constraint_point.longer_path_agent, constraint_point.constraint)
-        child_node2.parent_idx = node_index
-
-        heapq.heappush(self.nodes_to_expand, child_node1)
-        heapq.heappush(self.nodes_to_expand, child_node2)
+        heapq.heappush(self.nodes_to_expand, node)
 
-    def get_ancestor_constraints(self, parent_index: int):
+    def get_ancestor_constraints(self, parent_index: int) -> list[AppliedConstraint]:
         """
-        Get the constraints that were applied to the parent node to generate this node.
+        Get the constraints that were applied to all parent nodes starting with the node at the provided parent_index.
         """
-        constraints = []
+        constraints: list[AppliedConstraint] = []
         while parent_index != -1:
             node = self.expanded_nodes[parent_index]
-            if node.constraint is not None:
+            if node.constraint and isinstance(node.constraint, AppliedConstraint):
                 constraints.append(node.constraint)
+            else:
+                print(f"Aha!!! {node.constraint}")
             parent_index = node.parent_idx
         return constraints
+
+    def add_expanded_node(self, node: ConstraintTreeNode) -> int:
+        """
+        Add an expanded node to the tree. Returns the index of this node in the expanded nodes dictionary.
+        """
+        parent_idx = len(self.expanded_nodes)
+        self.expanded_nodes[parent_idx] = node
+        return parent_idx

PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

@@ -8,6 +8,7 @@
 from enum import Enum
 from dataclasses import dataclass
 from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position
+from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AppliedConstraint
 
 @dataclass
 class Interval:
@@ -31,6 +32,12 @@ def empty_2d_array_of_lists(x: int, y: int) -> np.ndarray:
     arr[:] = [[[] for _ in range(y)] for _ in range(x)]
     return arr
 
+def empty_3d_array_of_sets(x: int, y: int, z: int) -> np.ndarray:
+    arr = np.empty((x, y, z), dtype=object)
+    # assign each element individually - np.full creates references to the same list
+    arr[:] = [[[set() for _ in range(z)] for _ in range(y)] for _ in range(x)]
+    return arr
+
 class Grid:
     # Set in constructor
     grid_size: np.ndarray
@@ -39,6 +46,9 @@ class Grid:
     # Obstacles will never occupy these points. Useful to avoid impossible scenarios
     obstacle_avoid_points: list[Position] = []
 
+    # TODO: do i want this as part of grid?
+    constraint_points: np.ndarray
+
     # Number of time steps in the simulation
     time_limit: int
 
@@ -58,6 +68,8 @@ def __init__(
         self.grid_size = grid_size
         self.reservation_matrix = np.zeros((grid_size[0], grid_size[1], self.time_limit))
 
+        self.constraint_points = empty_3d_array_of_sets(grid_size[0], grid_size[1], self.time_limit)
+
         if num_obstacles > self.grid_size[0] * self.grid_size[1]:
             raise Exception("Number of obstacles is greater than grid size!")
 
@@ -185,6 +197,19 @@ def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Positi
 
         return obstacle_paths
 
+    def apply_constraint_points(self, constraints: list[AppliedConstraint], verbose = False):
+        for constraint in constraints:
+            if verbose:
+                print(f"Applying {constraint=}")
+            if constraint not in self.constraint_points[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]:
+                self.constraint_points[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time].add(constraint)
+
+            if verbose:
+                print(f"\tExisting constraints: {self.constraint_points[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]}")
+
+    def clear_constraint_points(self):
+        self.constraint_points = empty_3d_array_of_sets(self.grid_size[0], self.grid_size[1], self.time_limit)
+
     """
     Check if the given position is valid at time t
 
@@ -195,11 +220,19 @@ def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Positi
     output:
         bool: True if position/time combination is valid, False otherwise
     """
-    def valid_position(self, position: Position, t: int) -> bool:
+    def valid_position(self, position: Position, t: int, agent_idx: int) -> bool:
         # Check if position is in grid
         if not self.inside_grid_bounds(position):
             return False
 
+        constraints = self.constraint_points[position.x, position.y, t]
+        for constraint in constraints:
+            if constraint.constrained_agent == agent_idx:
+                return False
+
+        # if any([constraint.constrained_agent == agent_idx for constraint in constraints]):
+        #     return False
+
         # Check if position is not occupied at time t
         return self.reservation_matrix[position.x, position.y, t] == 0