wip - proved out some simple cases

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -33,19 +33,24 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
 
         initial_solution: dict[AgentId, NodePath] = {}
 
-        # Reserve initial positions
+        # Generate initial solution (no reservations for robots)
         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
 
+        # for (agent_idx, path) in initial_solution.items():
+        #     print(f"\nAgent {agent_idx} path:\n {path}")
+
         constraint_tree = ConstraintTree(initial_solution)
 
+        attempted_constraint_combos = set()
+
         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}")
+
+            # 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} ")
@@ -66,48 +71,96 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
                 if verbose:
                     print(f"\nOuter loop step for agent {constrained_agent}")
 
-                applied_constraint = AppliedConstraint(constraint_tree_node.constraint.constraint, constrained_agent)
-                all_constraints = deepcopy(ancestor_constraints)
+                applied_constraint = AppliedConstraint(constrained_agent.constraint, constrained_agent.agent)
+
+                # TODO: check type of other constraints somewhere
+                all_constraints = deepcopy(ancestor_constraints) # TODO - no deepcopy pls
                 all_constraints.append(applied_constraint)
 
+                # Skip if we have already tried this set of constraints
+                constraint_hash = hash(frozenset(all_constraints))
+                if constraint_hash in attempted_constraint_combos:
+                    break
+                else:
+                    attempted_constraint_combos.add(constraint_hash)
+
                 if verbose:
                     print(f"\tall constraints: {all_constraints}")
 
                 grid.clear_constraint_points()
                 grid.apply_constraint_points(all_constraints)
 
+                failed_to_plan = False
                 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)
+                    # print("planning for:", agent_idx)
+                    try:
+                        path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, agent_idx, verbose)
+                    except Exception as e:
+                        print(f"Failed to plan with constraints: {all_constraints}")
+                        failed_to_plan = True
+
                     if path is None:
                         raise RuntimeError(f"Failed to find path for {start_and_goal}")
                     paths[AgentId(start_and_goal.index)] = path
 
+                if failed_to_plan:
+                    print(f"Failed to plan with constraints: {all_constraints}")
+                    continue
+
+                # for (agent_idx, path) in paths.items():
+                #     print(f"\nAgent {agent_idx} path:\n {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)
 
                 new_constraint_tree_node = ConstraintTreeNode(paths, parent_idx)
                 if new_constraint_tree_node.constraint is None:
                     # This means we found a solution!
+                    print("Found a path with constraints:")
+                    for constraint in all_constraints:
+                        print(f"\t{constraint}")
                     return (start_and_goals, [paths[AgentId(i)] for i in range(len(start_and_goals))])
 
-                if verbose:
-                    print(f"Adding new constraint tree node with constraint: {new_constraint_tree_node.constraint}")
+                # 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)
 
+        raise RuntimeError("No solution found")
+# TODO
+# * get SIPP working w CBS
+# * add checks for duplicate expansions
+# * fan out across multiple threads
+# * sipp intervals seem much larger than needed?
+
 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, 8+i), Position(19, 19-i)) for i in range(4)]
+    # TODO: bug somewhere where it expects agent ids to match indices
+    # start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 12)]
+    start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(6)]
+    # start_and_goals = [StartAndGoal(i, Position(1, 2*i), Position(19, 19-i)) for i in range(4)]
+
+    # hallway cross
+    # start_and_goals = [StartAndGoal(0, Position(6, 10), Position(13, 10)),
+    #                    StartAndGoal(1, Position(13, 10), Position(7, 10)),
+    #                    StartAndGoal(2, Position(11, 10), Position(6, 10))]
+
+    # temporary obstacle
+    # start_and_goals = [StartAndGoal(0, Position(15, 14), Position(15, 16))]
+
+    # start_and_goals = [StartAndGoal(1, Position(6, 10), Position(8, 10)),
+    #                    StartAndGoal(2, Position(13, 10), Position(11, 10))]
     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.TEMPORARY_OBSTACLE,
+        # obstacle_arrangement=ObstacleArrangement.HALLWAY,
         obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
         # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
         # obstacle_arrangement=ObstacleArrangement.RANDOM,

PathPlanning/TimeBasedPathPlanning/ConstraintTree.py

@@ -12,9 +12,13 @@ class Constraint:
     time: int
 
 @dataclass
-class ForkingConstraint:
+class ConstrainedAgent:
+    agent: AgentId
     constraint: Constraint
-    constrained_agents: tuple[AgentId, AgentId]
+
+@dataclass
+class ForkingConstraint:
+    constrained_agents: tuple[ConstrainedAgent, ConstrainedAgent]
 
 @dataclass(frozen=True)
 class AppliedConstraint:
@@ -39,30 +43,53 @@ def __lt__(self, other) -> bool:
         return self.cost < other.cost
 
     def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]:
-        if verbose:
-            print(f"\tpath for {agent_id}: {path}\n")
 
         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():
+                # Check for edge conflicts
+                last_position = None
+                if t > 0:
+                    last_position = path.get_position(t - 1)
+
                 position = path.get_position(t)
                 if position is None:
                     continue
-                # print(f"reserving pos/t for {agent_id}: {position} @ {t}")
+                # print(f"\treserving 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:
+                    # if True:
                         print(f"found constraint: {position_at_time} for agents {agent_id} & {conflicting_agent_id}")
-                    return ForkingConstraint(
-                        constraint=Constraint(position=position, time=t),
-                        constrained_agents=(AgentId(agent_id), AgentId(conflicting_agent_id))
-                    )
+                    constraint = Constraint(position=position, time=t)
+                    return ForkingConstraint((
+                        ConstrainedAgent(agent_id, constraint), ConstrainedAgent(conflicting_agent_id, constraint)
+                    ))
                 else:
                     positions_at_time[position_at_time] = AgentId(agent_id)
+                
+                if last_position:
+                    new_position_at_last_time = PositionAtTime(position, t-1)
+                    old_position_at_new_time = PositionAtTime(last_position, t)
+                    if new_position_at_last_time in positions_at_time and old_position_at_new_time in positions_at_time:
+                        conflicting_agent_id1 = positions_at_time[new_position_at_last_time]
+                        conflicting_agent_id2 = positions_at_time[old_position_at_new_time]
+                        
+                        if conflicting_agent_id1 == conflicting_agent_id2 and conflicting_agent_id1 != agent_id:
+                            print(f"Found edge constraint between with agent {conflicting_agent_id1} for {agent_id}")
+                            print(f"\tpositions old: {old_position_at_new_time}, new: {position_at_time}")
+                            new_constraint = ForkingConstraint((
+                                ConstrainedAgent(agent_id, position_at_time),
+                                ConstrainedAgent(conflicting_agent_id1, Constraint(position=last_position, time=t))
+                            ))
+                            print(f"new constraint: {new_constraint}")
+                            return new_constraint
+                    else:
+                        positions_at_time[new_position_at_last_time] = AgentId(agent_id)
         return None
 
 

PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

@@ -22,14 +22,21 @@ class ObstacleArrangement(Enum):
     ARRANGEMENT1 = 1
     # Static obstacle arrangement
     NARROW_CORRIDOR = 2
+    # A hallway surrounded by obstacles with a 2-cell opening in the middle
+    HALLWAY = 3
+    # A temporary obstacle that vanishes after some time
+    TEMPORARY_OBSTACLE = 4
 
 """
 Generates a 2d numpy array with lists for elements.
 """
 def empty_2d_array_of_lists(x: int, y: int) -> np.ndarray:
     arr = np.empty((x, y), dtype=object)
     # assign each element individually - np.full creates references to the same list
-    arr[:] = [[[] for _ in range(y)] for _ in range(x)]
+    # arr[:] = [[[] for _ in range(y)] for _ in range(x)]
+    for x in range(arr.shape[0]):
+        for y in range(arr.shape[1]):
+            arr[x, y] = []
     return arr
 
 def empty_3d_array_of_sets(x: int, y: int, z: int) -> np.ndarray:
@@ -79,6 +86,10 @@ def __init__(
             self.obstacle_paths = self.obstacle_arrangement_1(num_obstacles)
         elif obstacle_arrangement == ObstacleArrangement.NARROW_CORRIDOR:
             self.obstacle_paths = self.generate_narrow_corridor_obstacles(num_obstacles)
+        elif obstacle_arrangement == ObstacleArrangement.HALLWAY:
+            self.obstacle_paths = self.generate_hallway_obstacles()
+        elif obstacle_arrangement == ObstacleArrangement.TEMPORARY_OBSTACLE:
+            self.obstacle_paths = self.generate_temporary_obstacle()
 
         for i, path in enumerate(self.obstacle_paths):
             # TODO: i think this is a bug. obstacle indices will overlap with robot indices
@@ -89,6 +100,19 @@ def __init__(
                     self.reservation_matrix[path[t - 1].x, path[t - 1].y, t] = obs_idx
                 self.reservation_matrix[position.x, position.y, t] = obs_idx
 
+    def reset(self):
+        self.reservation_matrix = np.zeros((self.grid_size[0], self.grid_size[1], self.time_limit))
+
+        # TODO: copy pasta from above
+        for i, path in enumerate(self.obstacle_paths):
+            # TODO: i think this is a bug. obstacle indices will overlap with robot indices
+            obs_idx = i + 1  # avoid using 0 - that indicates free space in the grid
+            for t, position in enumerate(path):
+                # Reserve old & new position at this time step
+                if t > 0:
+                    self.reservation_matrix[path[t - 1].x, path[t - 1].y, t] = obs_idx
+                self.reservation_matrix[position.x, position.y, t] = obs_idx
+
     """
     Generate dynamic obstacles that move around the grid. Initial positions and movements are random
     """
@@ -198,6 +222,55 @@ def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Positi
 
         return obstacle_paths
 
+
+    def generate_hallway_obstacles(self) -> list[list[Position]]:
+        """
+        Generate obstacles that form a hallway with a 2-cell opening in the middle.
+        Creates only a 1-cell border around the edges and hallway walls.
+        
+        Pattern created:
+        **********
+        *        *
+        ***    ***
+        **********
+        
+        Args:
+            hallway_length: Length of the hallway (number of rows for the corridor)
+        
+        Returns:
+            List of obstacle paths, where each path represents one obstacle over time
+        """
+        obstacle_paths = []
+        
+        for y in range(8, 12):
+            for x in range(5, 15):
+                is_obstacle = False
+                
+                # Border walls
+                if x == 5 or x == 14 or y == 8 or y == 11:
+                    is_obstacle = True
+                if y == 9 and x not in [9, 10]:
+                    is_obstacle = True
+                
+                # If this position should be an obstacle, create a path for it
+                if is_obstacle:
+                    obstacle_path = []
+                    for t in range(self.time_limit):
+                        obstacle_path.append(Position(x, y))
+                    obstacle_paths.append(obstacle_path)
+        
+        return obstacle_paths
+    
+    def generate_temporary_obstacle(self, hallway_length: int = 3) -> list[list[Position]]:
+        """
+        Generates a temporary obstacle at (10, 10) that disappears at t=30
+        """
+        obstacle_path = []
+        for t in range(max(self.time_limit, 40)):
+            obstacle_path.append(Position(15, 15))
+        
+        return [obstacle_path]
+
     def apply_constraint_points(self, constraints: list[AppliedConstraint], verbose = False):
         for constraint in constraints:
             if verbose:
@@ -299,7 +372,7 @@ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Int
         vals = self.reservation_matrix[cell.x, cell.y, :]
 
         had_constraints = False
-        # ct: AppliedConstraint
+
         for constraint_set in self.constraint_points[cell.x, cell.y]:
             for constraint in constraint_set:
                 if constraint.constrained_agent != agent_idx:
@@ -308,10 +381,12 @@ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Int
                     continue
                 had_constraints = True
                 vals[constraint.constraint.time] = 99999 # TODO: no magic numbers
-                
+                # print(f"\tapplying constraint at cell {cell}: {constraint}")
                 # TODO: hack
-                # if constraint.constraint.time + 1 < self.time_limit:
-                #     vals[constraint.constraint.time + 1] = 99999 # TODO: no magic numbers
+                # if constraint.constraint.time + 3 < self.time_limit:
+                    # vals[constraint.constraint.time + 1] = 99999 # TODO: no magic numbers
+                    # vals[constraint.constraint.time + 2] = 99999 # TODO: no magic numbers
+                    # vals[constraint.constraint.time + 3] = 99999 # TODO: no magic numbers
 
         # Find where the array is zero
         zero_mask = (vals == 0)
@@ -340,8 +415,9 @@ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Int
         intervals = [interval for interval in intervals if interval.start_time != interval.end_time]
 
         # if had_constraints:
-            # print("\t\tconstraints: ", self.constraint_points[cell.x, cell.y])
-            # print("\t\tintervals: ", intervals)
+        #     print(f"agent {agent_idx}")
+        #     print("\t\tconstraints: ", self.constraint_points[cell.x, cell.y])
+        #     print("\t\tintervals: ", intervals)
         return intervals
 
     """
@@ -352,6 +428,7 @@ def reserve_path(self, node_path: NodePath, agent_index: int):
         if agent_index == 0:
             raise Exception("Agent index cannot be 0")
 
+        # TODO: this is wrong for SIPP
         for i, node in enumerate(node_path.path):
             reservation_finish_time = node.time + 1
             if i < len(node_path.path) - 1: