found real dumb bug in SIPP

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -38,8 +38,10 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
             path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.index, verbose)
             initial_solution[AgentId(start_and_goal.index)] = path
 
-        for (agent_idx, path) in initial_solution.items():
-            print(f"\nAgent {agent_idx} path:\n {path}")
+        if verbose:
+            print("Initial solution:")
+            for (agent_idx, path) in initial_solution.items():
+                print(f"\nAgent {agent_idx} path:\n {path}")
 
         constraint_tree = ConstraintTree(initial_solution)
 
@@ -49,8 +51,8 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
             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,6 +68,9 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
 
             # TODO: contents of this loop should probably be in a helper?
             for constrained_agent in constraint_tree_node.constraint.constrained_agents:
+                num_expansions = constraint_tree.expanded_node_count()
+                if num_expansions % 50 == 0:
+                    print(f"Expanded {num_expansions} nodes so far...")
                 if verbose:
                     print(f"\nOuter loop step for agent {constrained_agent}")
 
@@ -78,40 +83,41 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
                 # Skip if we have already tried this set of constraints
                 constraint_hash = hash(frozenset(all_constraints))
                 if constraint_hash in attempted_constraint_combos:
-                    print(f"\tSkipping already attempted constraint combination: {all_constraints}")
+                    if verbose:
+                        print(f"\tSkipping already attempted constraint combination: {all_constraints}")
                     continue
                 else:
                     attempted_constraint_combos.add(constraint_hash)
 
                 if verbose:
                     print(f"\tall constraints: {all_constraints}")
-                print(f"\tall constraints: {all_constraints}")
 
                 grid.clear_constraint_points()
                 grid.apply_constraint_points(all_constraints)
 
                 # Just plan for agent with new constraint
                 start_and_goal = ConflictBasedSearch.find_by_index(start_and_goals, constrained_agent.agent)
                 try:
-                    print("\tplanning for: {}", start_and_goal)
+                    if verbose:
+                        print("\tplanning for: {}", start_and_goal)
                     new_path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.index, verbose)
                 except Exception as e:
                     continue
 
                 applied_constraint_parent = deepcopy(constraint_tree_node) #TODO: not sure if deepcopy is actually needed
-                paths: dict[AgentId, NodePath] = deepcopy(applied_constraint_parent.paths) # TODO: not sure if deepcopy is actually needed
+                paths: dict[AgentId, NodePath] = deepcopy(constraint_tree_node.paths) # TODO: not sure if deepcopy is actually needed
                 paths[constrained_agent.agent] = new_path
 
-                for (agent_idx, path) in paths.items():
-                    print(f"\nAgent {agent_idx} path:\n {path}")
+                # for (agent_idx, path) in paths.items():
+                #     print(f"\nAgent {agent_idx} path:\n {path}")
 
                 applied_constraint_parent.constraint = applied_constraint
                 parent_idx = constraint_tree.add_expanded_node(applied_constraint_parent)
 
-                new_constraint_tree_node = ConstraintTreeNode(deepcopy(paths), parent_idx, all_constraints)
+                new_constraint_tree_node = ConstraintTreeNode(paths, parent_idx, all_constraints)
                 if new_constraint_tree_node.constraint is None:
                     # This means we found a solution!
-                    print("Found a path with constraints:")
+                    print(f"Found a path with constraints after {num_expansions} expansions:")
                     for constraint in all_constraints:
                         print(f"\t{constraint}")
                     # return (start_and_goals, [paths[AgentId(i)] for i in range(len(start_and_goals))])
@@ -131,25 +137,24 @@ def find_by_index(start_and_goal_list: list[StartAndGoal], target_index: AgentId
         raise RuntimeError(f"Could not find agent with index {target_index} in {start_and_goal_list}")
 
 # TODO
-# * get SIPP working w CBS
-# * add checks for duplicate expansions
+# * still discrepancies between sipp and A*
 # * fan out across multiple threads
-# * sipp intervals seem much larger than needed?
-
+# * somehow test/check that high level tree is doing what you want
+# * SIPP stinks at 3 robots in the hallway case
 verbose = False
 show_animation = True
 def main():
     grid_side_length = 21
 
     # 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, 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(11, 10), Position(6, 10)),
-                       StartAndGoal(2, Position(13, 10), Position(7, 10))]
+    # start_and_goals = [StartAndGoal(0, Position(6, 10), Position(13, 10)),
+    #                    StartAndGoal(1, Position(11, 10), Position(6, 10)),
+    #                    StartAndGoal(2, Position(13, 10), Position(7, 10))]
 
     # temporary obstacle
     # start_and_goals = [StartAndGoal(0, Position(15, 14), Position(15, 16))]
@@ -163,8 +168,8 @@ def main():
         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.HALLWAY,
+        obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
         # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
         # obstacle_arrangement=ObstacleArrangement.RANDOM,
     )

PathPlanning/TimeBasedPathPlanning/ConstraintTree.py

@@ -75,8 +75,8 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]:
                         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}")
+                            # 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))
@@ -94,14 +94,17 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]:
                     )))
                     continue
             if possible_constraints:
-                print(f"choosing best constraint of {possible_constraints}")
+                if verbose:
+                    print(f"Choosing best constraint of {possible_constraints}")
                 # first check for edge constraints
                 for constraint in possible_constraints:
                     if constraint.constrained_agents[0].constraint.position != constraint.constrained_agents[1].constraint.position:
-                        print(f"\tfound edge conflict constraint: {constraint}")
+                        if verbose:
+                            print(f"\tFound edge conflict constraint: {constraint}")
                         return constraint
                 # if none, then return first normal constraint
-                print("\treturning normal constraint")
+                if verbose:
+                    print(f"\tReturning normal constraint: {possible_constraints[0]}")
                 return possible_constraints[0]
 
         return None
@@ -150,3 +153,6 @@ def add_expanded_node(self, node: ConstraintTreeNode) -> int:
         parent_idx = len(self.expanded_nodes)
         self.expanded_nodes[parent_idx] = node
         return parent_idx
+
+    def expanded_node_count(self) -> int:
+        return len(self.expanded_nodes)

PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

@@ -55,6 +55,7 @@ class Grid:
 
     # TODO: do i want this as part of grid?
     constraint_points: np.ndarray
+    applied_constraints: list[AppliedConstraint] = []
 
     # Number of time steps in the simulation
     time_limit: int
@@ -272,6 +273,7 @@ def generate_temporary_obstacle(self, hallway_length: int = 3) -> list[list[Posi
         return [obstacle_path]
 
     def apply_constraint_points(self, constraints: list[AppliedConstraint], verbose = False):
+        self.applied_constraints.extend(constraints)
         for constraint in constraints:
             if verbose:
                 print(f"Applying {constraint=}")
@@ -282,7 +284,10 @@ def apply_constraint_points(self, constraints: list[AppliedConstraint], 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)
+        for constraint in self.applied_constraints:
+            if constraint 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].remove(constraint)
+        self.applied_constraints.clear()
 
     """
     Check if the given position is valid at time t
@@ -304,9 +309,6 @@ def valid_position(self, position: Position, t: int, agent_idx: int) -> bool:
             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
 

PathPlanning/TimeBasedPathPlanning/SafeInterval.py

@@ -63,7 +63,6 @@ class SafeIntervalPathPlanner(SingleAgentPlanner):
     """
     @staticmethod
     def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: bool = False) -> NodePath:
-
         # TODO: hacky
         grid.reset()
         safe_intervals = grid.get_safe_intervals(agent_idx)
@@ -134,8 +133,8 @@ def generate_successors(
                 continue
 
             current_interval = parent_node.interval
-
             new_cell_intervals: list[Interval] = intervals[new_pos.x, new_pos.y]
+
             for interval in new_cell_intervals:
                 # if interval starts after current ends, break
                 # assumption: intervals are sorted by start time, so all future intervals will hit this condition as well
@@ -157,7 +156,9 @@ def generate_successors(
 
                 # We know there is a node worth expanding. Generate successor at the earliest possible time the
                 # new interval can be entered
-                for possible_t in range(max(parent_node.time + 1, interval.start_time), min(current_interval.end_time, interval.end_time)):
+                minimum_entry_time = max(parent_node.time + 1, interval.start_time)
+                maximum_entry_time = min(current_interval.end_time, interval.end_time)
+                for possible_t in range(minimum_entry_time, maximum_entry_time + 1):
                     if grid.valid_position(new_pos, possible_t, agent_idx):
                         new_nodes.append(SIPPNode(
                             new_pos,