From aeca90c26e38d65001508d2025c36dd4ce68fd9e Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 10 Aug 2025 12:07:18 -0400 Subject: [PATCH 01/29] started sketching solution --- .../ConflictBasedSearch.py | 100 +++++++++++++++++ .../TimeBasedPathPlanning/ConstraintTree.py | 106 ++++++++++++++++++ 2 files changed, 206 insertions(+) create mode 100644 PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py create mode 100644 PathPlanning/TimeBasedPathPlanning/ConstraintTree.py diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py new file mode 100644 index 0000000000..b68ebd7fba --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -0,0 +1,100 @@ +""" +TODO - docstring + +Algorithm is defined in this paper: https://cdn.aaai.org/ojs/8140/8140-13-11667-1-2-20201228.pdf +""" + +import numpy as np +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Grid, + Interval, + ObstacleArrangement, + Position, +) +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.Plotting import PlotNodePaths +import time + +class ConflictBasedSearch(MultiAgentPlanner): + + @staticmethod + def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]: + """ + Generate a path from the start to the goal for each agent in the `start_and_goals` list. + Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans + corresponds to the order of the `start_and_goals` list. + """ + print(f"Using single-agent planner: {single_agent_planner_class}") + + # 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)) + + # 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) + + # first, plan optimally for each agent + # now in a loop: + # * + + # paths = [] + # for start_and_goal in start_and_goals: + # if verbose: + # print(f"\nPlanning for agent: {start_and_goal}" ) + + # 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) + + # if path is None: + # print(f"Failed to find path for {start_and_goal}") + # return [] + + # agent_index = start_and_goal.index + # grid.reserve_path(path, agent_index) + # paths.append(path) + + return (start_and_goals, paths) + + def cbs(): + +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)] + 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.RANDOM, + ) + + start_time = time.time() + start_and_goals, paths = ConflictBasedSearch.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) + + runtime = time.time() - start_time + print(f"\nPlanning took: {runtime:.5f} seconds") + + if verbose: + print(f"Paths:") + for path in paths: + print(f"{path}\n") + + if not show_animation: + return + + PlotNodePaths(grid, start_and_goals, paths) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py new file mode 100644 index 0000000000..f235901137 --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -0,0 +1,106 @@ +from dataclasses import dataclass +from typing import Optional, TypeAlias +import heapq + +from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position + +AgentId: TypeAlias = int + +@dataclass +class Constraint: + position: Position + time: int + +@dataclass +class PathConstraint: + constraint: Constraint + shorter_path_agent: AgentId + longer_path_agent: AgentId + +@dataclass +class ConstraintTreeNode: + parent_idx = int + constraint: tuple[AgentId, Constraint] + + paths: dict[AgentId, NodePath] + cost: int + + def __lt__(self, other) -> bool: + # TODO - this feels jank? + return self.cost + self.constrained_path_cost() < other.cost + other.constrained_path_cost() + + 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] = {} + 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( + 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 + ) + 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 + + 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)) + + def get_next_node_to_expand(self) -> Optional[ConstraintTreeNode]: + if not self.nodes_to_expand: + return None + return heapq.heappop(self.nodes_to_expand) + + 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) + + def get_ancestor_constraints(self, parent_index: int): + """ + Get the constraints that were applied to the parent node to generate this node. + """ + constraints = [] + while parent_index != -1: + node = self.expanded_nodes[parent_index] + if node.constraint is not None: + constraints.append(node.constraint) + parent_index = node.parent_idx + return constraints From 54b7ebc085b1b407afd88cfec02cc74f4e0860cc Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 10 Aug 2025 19:59:11 -0400 Subject: [PATCH 02/29] working, but slow. maybe duplicate constraint sets in the outer tree? --- .../TimeBasedPathPlanning/BaseClasses.py | 2 +- .../ConflictBasedSearch.py | 91 +++++++++++----- .../TimeBasedPathPlanning/ConstraintTree.py | 101 +++++++++--------- .../GridWithDynamicObstacles.py | 35 +++++- PathPlanning/TimeBasedPathPlanning/Node.py | 11 ++ .../TimeBasedPathPlanning/SafeInterval.py | 8 +- .../TimeBasedPathPlanning/SpaceTimeAStar.py | 8 +- 7 files changed, 169 insertions(+), 87 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 745cde45fb..7c819656c6 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/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 diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index b68ebd7fba..17bf964552 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/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") diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index f235901137..0b0122c511 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/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 diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index ccc2989001..20e4ba8cbc 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/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 diff --git a/PathPlanning/TimeBasedPathPlanning/Node.py b/PathPlanning/TimeBasedPathPlanning/Node.py index 728eebb676..f311319d39 100644 --- a/PathPlanning/TimeBasedPathPlanning/Node.py +++ b/PathPlanning/TimeBasedPathPlanning/Node.py @@ -28,6 +28,17 @@ def __sub__(self, other): def __hash__(self): return hash((self.x, self.y)) +@dataclass(order=True) +class PositionAtTime: + position: Position + time: int + + def __hash__(self): + return hash((self.position, self.time)) + + def __eq__(self, other): + return self.position == other.position and self.time == other.time + @dataclass() # Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because # this class needs to override the __lt__ and __eq__ methods to ignore parent_index. Parent diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index 446847ac6d..8984c790c8 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -47,7 +47,7 @@ class SafeIntervalPathPlanner(SingleAgentPlanner): verbose (bool): set to True to print debug information """ @staticmethod - 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: safe_intervals = grid.get_safe_intervals() @@ -90,7 +90,7 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> entry_time_and_node = EntryTimeAndInterval(expanded_node.time, expanded_node.interval) add_entry_to_visited_intervals_array(entry_time_and_node, visited_intervals, expanded_node) - for child in SafeIntervalPathPlanner.generate_successors(grid, goal, expanded_node, expanded_idx, safe_intervals, visited_intervals): + for child in SafeIntervalPathPlanner.generate_successors(grid, goal, expanded_node, expanded_idx, safe_intervals, visited_intervals, agent_idx): heapq.heappush(open_set, child) raise Exception("No path found") @@ -100,7 +100,7 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> """ @staticmethod def generate_successors( - grid: Grid, goal: Position, parent_node: SIPPNode, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray + grid: Grid, goal: Position, parent_node: SIPPNode, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray, agent_idx: int ) -> list[SIPPNode]: new_nodes = [] diffs = [ @@ -140,7 +140,7 @@ 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)): - if grid.valid_position(new_pos, possible_t): + if grid.valid_position(new_pos, possible_t, agent_idx): new_nodes.append(SIPPNode( new_pos, # entry is max of interval start and parent node time + 1 (get there as soon as possible) diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index b85569f5dc..5136c557c1 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -24,7 +24,7 @@ class SpaceTimeAStar(SingleAgentPlanner): @staticmethod - def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath: + def plan(grid: Grid, start: Position, goal: Position, agent_index: int, verbose: bool = False) -> NodePath: open_set: list[Node] = [] heapq.heappush( open_set, Node(start, 0, SpaceTimeAStar.calculate_heuristic(start, goal), -1) @@ -62,7 +62,7 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> expanded_list.append(expanded_node) expanded_set.add(expanded_node) - for child in SpaceTimeAStar.generate_successors(grid, goal, expanded_node, expanded_idx, verbose, expanded_set): + for child in SpaceTimeAStar.generate_successors(grid, goal, expanded_node, expanded_idx, agent_index, verbose, expanded_set): heapq.heappush(open_set, child) raise Exception("No path found") @@ -72,7 +72,7 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> """ @staticmethod def generate_successors( - grid: Grid, goal: Position, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node] + grid: Grid, goal: Position, parent_node: Node, parent_node_idx: int, agent_index: int, verbose: bool, expanded_set: set[Node] ) -> Generator[Node, None, None]: diffs = [ Position(0, 0), @@ -94,7 +94,7 @@ def generate_successors( continue # Check if the new node is valid for the next 2 time steps - one step to enter, and another to leave - if all([grid.valid_position(new_pos, parent_node.time + dt) for dt in [1, 2]]): + if all([grid.valid_position(new_pos, parent_node.time + dt, agent_index) for dt in [1, 2]]): if verbose: print("\tNew successor node: ", new_node) yield new_node From b6e074902407167e3758399365ff4f2bc93c8598 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 10 Aug 2025 20:34:56 -0400 Subject: [PATCH 03/29] debugging sipp, not sure. this commit is mostly garbo --- .../ConflictBasedSearch.py | 5 ++-- .../GridWithDynamicObstacles.py | 27 ++++++++++++++++--- .../TimeBasedPathPlanning/SafeInterval.py | 2 +- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 17bf964552..0464d18cca 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -101,7 +101,7 @@ 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(5)] + start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(4)] obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)] grid = Grid( @@ -114,7 +114,8 @@ def main(): ) start_time = time.time() - start_and_goals, paths = ConflictBasedSearch.plan(grid, start_and_goals, SpaceTimeAStar, verbose) + 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") diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 20e4ba8cbc..cdff91e45f 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -81,6 +81,7 @@ def __init__( self.obstacle_paths = self.generate_narrow_corridor_obstacles(num_obstacles) 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 @@ -282,11 +283,11 @@ def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]: """ Returns safe intervals for each cell. """ - def get_safe_intervals(self) -> np.ndarray: + def get_safe_intervals(self, agent_idx: int) -> np.ndarray: intervals = empty_2d_array_of_lists(self.grid_size[0], self.grid_size[1]) for x in range(intervals.shape[0]): for y in range(intervals.shape[1]): - intervals[x, y] = self.get_safe_intervals_at_cell(Position(x, y)) + intervals[x, y] = self.get_safe_intervals_at_cell(Position(x, y), agent_idx) return intervals @@ -294,8 +295,24 @@ def get_safe_intervals(self) -> np.ndarray: Generate the safe intervals for a given cell. The intervals will be in order of start time. ex: Interval (2, 3) will be before Interval (4, 5) """ - def get_safe_intervals_at_cell(self, cell: Position) -> list[Interval]: + def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Interval]: 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: + continue + if constraint.constraint.position != cell: + continue + had_constraints = True + vals[constraint.constraint.time] = 99999 # TODO: no magic numbers + + # TODO: hack + # if constraint.constraint.time + 1 < self.time_limit: + # vals[constraint.constraint.time + 1] = 99999 # TODO: no magic numbers + # Find where the array is zero zero_mask = (vals == 0) @@ -321,6 +338,10 @@ def get_safe_intervals_at_cell(self, cell: Position) -> list[Interval]: # move into and out of the cell each take 1 time step, and the cell is considered occupied during # both the time step when it is entering the cell, and the time step when it is leaving the cell. 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) return intervals """ diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index 8984c790c8..c388b1d9c8 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -49,7 +49,7 @@ class SafeIntervalPathPlanner(SingleAgentPlanner): @staticmethod def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: bool = False) -> NodePath: - safe_intervals = grid.get_safe_intervals() + safe_intervals = grid.get_safe_intervals(agent_idx) open_set: list[SIPPNode] = [] first_node_interval = safe_intervals[start.x, start.y][0] From ac06806e581edb4cd85fe97575da8b545babaf12 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 16 Aug 2025 23:08:18 -0400 Subject: [PATCH 04/29] wip - proved out some simple cases --- .../ConflictBasedSearch.py | 75 ++++++++++++--- .../TimeBasedPathPlanning/ConstraintTree.py | 45 +++++++-- .../GridWithDynamicObstacles.py | 91 +++++++++++++++++-- .../TimeBasedPathPlanning/SafeInterval.py | 23 ++++- 4 files changed, 204 insertions(+), 30 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 0464d18cca..84ef6f4543 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/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,22 +71,45 @@ 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) @@ -89,25 +117,50 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c 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, diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 0b0122c511..295c099ae3 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/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 diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index cdff91e45f..17ab5fedbf 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -22,6 +22,10 @@ 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. @@ -29,7 +33,10 @@ class ObstacleArrangement(Enum): 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: diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index c388b1d9c8..95f31c23b7 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -24,6 +24,7 @@ from dataclasses import dataclass from functools import total_ordering import time +from typing import Sequence @dataclass() # Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because @@ -34,6 +35,20 @@ class SIPPNode(Node): interval: Interval +class SIPPNodePath(NodePath): + def __init__(self, path: Sequence[SIPPNode], expanded_node_count: int): + super().__init__(path, expanded_node_count) + + self.positions_at_time = {} + last_position = path[0].position + for t in range(0, path[-1].time + 1): + for node in path: + if node.time == t: + last_position = node.position + break + if node.time > t: + break + self.positions_at_time[t] = last_position @dataclass class EntryTimeAndInterval: entry_time: int @@ -49,8 +64,11 @@ 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) + open_set: list[SIPPNode] = [] first_node_interval = safe_intervals[start.x, start.y][0] heapq.heappush( @@ -83,7 +101,7 @@ def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: b # reverse path so it goes start -> goal path.reverse() - return NodePath(path, len(expanded_list)) + return SIPPNodePath(path, len(expanded_list)) expanded_idx = len(expanded_list) expanded_list.append(expanded_node) @@ -143,8 +161,7 @@ def generate_successors( if grid.valid_position(new_pos, possible_t, agent_idx): new_nodes.append(SIPPNode( new_pos, - # entry is max of interval start and parent node time + 1 (get there as soon as possible) - max(interval.start_time, parent_node.time + 1), + possible_t, SafeIntervalPathPlanner.calculate_heuristic(new_pos, goal), parent_node_idx, interval, From b05faa46a12c313a9cbac2caafff04d3f1eeffbb Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 16 Aug 2025 23:16:10 -0400 Subject: [PATCH 05/29] running difftest, passes in 85 seconds --- .../TimeBasedPathPlanning/ConflictBasedSearch.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 84ef6f4543..815022dd15 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -140,13 +140,13 @@ def main(): # 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(13, 10), Position(7, 10)), - # StartAndGoal(2, Position(11, 10), Position(6, 10))] + start_and_goals = [StartAndGoal(0, Position(6, 10), Position(13, 10)), + StartAndGoal(1, Position(12, 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))] @@ -160,8 +160,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, ) From 91a3468e49154e83b3230f3dec9b60bf823dcc3d Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 17 Aug 2025 00:49:14 -0400 Subject: [PATCH 06/29] some speedup + bugfix --- .../ConflictBasedSearch.py | 44 ++++++++----------- .../TimeBasedPathPlanning/ConstraintTree.py | 9 ++-- 2 files changed, 23 insertions(+), 30 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 815022dd15..fa1c4f991d 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -66,8 +66,6 @@ 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: - paths: dict[AgentId, NodePath] = {} - if verbose: print(f"\nOuter loop step for agent {constrained_agent}") @@ -90,37 +88,31 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c 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): - # 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}") + # Just plan for agent with new constraint + start_and_goal = start_and_goals[constrained_agent.agent] + try: + 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[constrained_agent.agent] = new_path + # 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) + new_constraint_tree_node = ConstraintTreeNode(deepcopy(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:") for constraint in all_constraints: print(f"\t{constraint}") - return (start_and_goals, [paths[AgentId(i)] for i in range(len(start_and_goals))]) + # return (start_and_goals, [paths[AgentId(i)] for i in range(len(start_and_goals))]) + return (start_and_goals, paths.values()) # if verbose: print(f"Adding new constraint tree node with constraint: {new_constraint_tree_node.constraint}") @@ -140,13 +132,13 @@ def main(): # 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(12, 10), Position(7, 10)), - StartAndGoal(2, Position(11, 10), Position(6, 10))] + # 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))] @@ -160,8 +152,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, ) diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 295c099ae3..6201b35f62 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -33,13 +33,16 @@ class ConstraintTreeNode: paths: dict[AgentId, NodePath] cost: int - def __init__(self, paths: dict[AgentId, NodePath], parent_idx: int): + def __init__(self, paths: dict[AgentId, NodePath], parent_idx: int, all_constraints: list[AppliedConstraint]): 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() + self.all_constraints = all_constraints def __lt__(self, other) -> bool: + if self.cost == other.cost: + return len(self.all_constraints) < len(other.all_constraints) return self.cost < other.cost def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]: @@ -88,8 +91,6 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]: )) print(f"new constraint: {new_constraint}") return new_constraint - else: - positions_at_time[new_position_at_last_time] = AgentId(agent_id) return None @@ -102,7 +103,7 @@ class ConstraintTree: def __init__(self, initial_solution: dict[AgentId, NodePath]): self.nodes_to_expand = [] self.expanded_nodes = {} - heapq.heappush(self.nodes_to_expand, ConstraintTreeNode(initial_solution, -1)) + 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: From 26e2d3801288ac535b6d4887fa2b82e4fba3312a Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 17 Aug 2025 16:54:28 -0400 Subject: [PATCH 07/29] some more debugging, fixed issues with agent id expecting in order + better constraint finding --- .../TimeBasedPathPlanning/BaseClasses.py | 1 + .../ConflictBasedSearch.py | 55 +++++++++++-------- .../TimeBasedPathPlanning/ConstraintTree.py | 39 ++++++++----- 3 files changed, 60 insertions(+), 35 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 7c819656c6..6f64fdd83b 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -26,6 +26,7 @@ def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: b @dataclass class StartAndGoal: # Index of this agent + # TODO: better name and use AgentId type index: int # Start position of the robot start: Position diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index fa1c4f991d..fa1c0e7d5c 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -34,12 +34,12 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c initial_solution: dict[AgentId, NodePath] = {} # Generate initial solution (no reservations for robots) - 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) - initial_solution[AgentId(agent_idx)] = path + for start_and_goal in start_and_goals: + 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}") + for (agent_idx, path) in initial_solution.items(): + print(f"\nAgent {agent_idx} path:\n {path}") constraint_tree = ConstraintTree(initial_solution) @@ -49,17 +49,17 @@ 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} ") + 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))]) + return (start_and_goals, [constraint_tree_node.paths[start_and_goal.index] for start_and_goal in start_and_goals]) if not isinstance(constraint_tree_node.constraint, ForkingConstraint): raise ValueError(f"Expected a ForkingConstraint, but got: {constraint_tree_node.constraint}") @@ -78,19 +78,22 @@ 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: - break + 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 = start_and_goals[constrained_agent.agent] + start_and_goal = ConflictBasedSearch.find_by_index(start_and_goals, constrained_agent.agent) try: + 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 @@ -99,8 +102,8 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c paths: dict[AgentId, NodePath] = deepcopy(applied_constraint_parent.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) @@ -115,10 +118,18 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c return (start_and_goals, paths.values()) # if verbose: - print(f"Adding new constraint tree node with constraint: {new_constraint_tree_node.constraint}") + # 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: bad function name + def find_by_index(start_and_goal_list: list[StartAndGoal], target_index: AgentId) -> AgentId: + for item in start_and_goal_list: + if item.index == target_index: + return item + 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 @@ -132,13 +143,13 @@ def main(): # 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(13, 10), Position(7, 10)), - # StartAndGoal(2, Position(11, 10), Position(6, 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))] @@ -152,15 +163,15 @@ 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, ) 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) + # 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") diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 6201b35f62..a94523488a 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -51,6 +51,7 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]: 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 + possible_constraints: list[ForkingConstraint] = [] for agent_id, path in self.paths.items(): # Check for edge conflicts last_position = None @@ -62,19 +63,10 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]: continue # 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}") - constraint = Constraint(position=position, time=t) - return ForkingConstraint(( - ConstrainedAgent(agent_id, constraint), ConstrainedAgent(conflicting_agent_id, constraint) - )) - else: + if position_at_time not in positions_at_time: positions_at_time[position_at_time] = AgentId(agent_id) + # edge conflict if last_position: new_position_at_last_time = PositionAtTime(position, t-1) old_position_at_new_time = PositionAtTime(last_position, t) @@ -89,8 +81,29 @@ def get_constraint_point(self, verbose = False) -> Optional[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 + possible_constraints.append(new_constraint) + continue + + # double reservation at a (cell, time) combination + if positions_at_time[position_at_time] != agent_id: + conflicting_agent_id = positions_at_time[position_at_time] + + constraint = Constraint(position=position, time=t) + possible_constraints.append(ForkingConstraint(( + ConstrainedAgent(agent_id, constraint), ConstrainedAgent(conflicting_agent_id, constraint) + ))) + continue + if possible_constraints: + 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}") + return constraint + # if none, then return first normal constraint + print("\treturning normal constraint") + return possible_constraints[0] + return None From ae26cea09f412e5bebf4d19ebc9861991f6c4de7 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 17 Aug 2025 18:32:31 -0400 Subject: [PATCH 08/29] found real dumb bug in SIPP --- .../ConflictBasedSearch.py | 49 ++++++++++--------- .../TimeBasedPathPlanning/ConstraintTree.py | 16 ++++-- .../GridWithDynamicObstacles.py | 10 ++-- .../TimeBasedPathPlanning/SafeInterval.py | 7 +-- 4 files changed, 48 insertions(+), 34 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index fa1c0e7d5c..27000827b2 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/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,14 +83,14 @@ 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) @@ -93,25 +98,26 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c # 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,11 +137,10 @@ 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(): @@ -143,13 +148,13 @@ def main(): # 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, ) diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index a94523488a..502f668b04 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/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) \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 17ab5fedbf..5a14053393 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/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 diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index 95f31c23b7..f16ed223fc 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/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, From 6c95e4d28be8e6c4204d9169758bfb02bd92f2f0 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 17 Aug 2025 19:59:51 -0400 Subject: [PATCH 09/29] add random positions start_and_goals --- .../ConflictBasedSearch.py | 29 ++++++++++++++----- .../TimeBasedPathPlanning/ConstraintTree.py | 2 +- .../GridWithDynamicObstacles.py | 14 +++++---- 3 files changed, 32 insertions(+), 13 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 27000827b2..d853f806b4 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -68,9 +68,6 @@ 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}") @@ -80,6 +77,11 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c all_constraints = deepcopy(ancestor_constraints) # TODO - no deepcopy pls all_constraints.append(applied_constraint) + num_expansions = constraint_tree.expanded_node_count() + if num_expansions % 50 == 0: + print(f"Expanded {num_expansions} nodes so far...") + print(f"\tlen of constraints {len(all_constraints)}") + # Skip if we have already tried this set of constraints constraint_hash = hash(frozenset(all_constraints)) if constraint_hash in attempted_constraint_combos: @@ -143,14 +145,26 @@ def find_by_index(start_and_goal_list: list[StartAndGoal], target_index: AgentId # * SIPP stinks at 3 robots in the hallway case verbose = False show_animation = True +np.random.seed(42) # For reproducibility 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)] + # generate random start and goals + start_and_goals: list[StartAndGoal] = [] + for i in range(40): + start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + while any([start_and_goal.start == start for start_and_goal in start_and_goals]): + start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + + start_and_goals.append(StartAndGoal(i, start, goal)) + # hallway cross # start_and_goals = [StartAndGoal(0, Position(6, 10), Position(13, 10)), # StartAndGoal(1, Position(11, 10), Position(6, 10)), @@ -169,14 +183,15 @@ def main(): obstacle_avoid_points=obstacle_avoid_points, # obstacle_arrangement=ObstacleArrangement.TEMPORARY_OBSTACLE, # obstacle_arrangement=ObstacleArrangement.HALLWAY, - obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR, + # obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR, + obstacle_arrangement=ObstacleArrangement.NONE, # 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) + 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") diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 502f668b04..af0f69447b 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -142,7 +142,7 @@ def get_ancestor_constraints(self, parent_index: int) -> list[AppliedConstraint] if node.constraint and isinstance(node.constraint, AppliedConstraint): constraints.append(node.constraint) else: - print(f"Aha!!! {node.constraint}") + raise RuntimeError(f"Expected AppliedConstraint, but got: {node.constraint}") parent_index = node.parent_idx return constraints diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 5a14053393..285a418fd0 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -16,16 +16,18 @@ class Interval: end_time: int class ObstacleArrangement(Enum): + # No obstacles + NONE = 0 # Random obstacle positions and movements - RANDOM = 0 + RANDOM = 1 # Obstacles start in a line in y at center of grid and move side-to-side in x - ARRANGEMENT1 = 1 + ARRANGEMENT1 = 2 # Static obstacle arrangement - NARROW_CORRIDOR = 2 + NARROW_CORRIDOR = 3 # A hallway surrounded by obstacles with a 2-cell opening in the middle - HALLWAY = 3 + HALLWAY = 4 # A temporary obstacle that vanishes after some time - TEMPORARY_OBSTACLE = 4 + TEMPORARY_OBSTACLE = 5 """ Generates a 2d numpy array with lists for elements. @@ -91,6 +93,8 @@ def __init__( self.obstacle_paths = self.generate_hallway_obstacles() elif obstacle_arrangement == ObstacleArrangement.TEMPORARY_OBSTACLE: self.obstacle_paths = self.generate_temporary_obstacle() + elif obstacle_arrangement == ObstacleArrangement.NONE: + self.obstacle_paths = [] for i, path in enumerate(self.obstacle_paths): # TODO: i think this is a bug. obstacle indices will overlap with robot indices From ea4b95a52e4591ecfbac9146b54892343a9d9b61 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Sat, 11 Oct 2025 18:39:58 -0400 Subject: [PATCH 10/29] some cleanup --- .../ConflictBasedSearch.py | 132 ++++++++++-------- .../TimeBasedPathPlanning/ConstraintTree.py | 6 +- 2 files changed, 74 insertions(+), 64 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index d853f806b4..0d4bd35d1a 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -10,6 +10,7 @@ ObstacleArrangement, Position, ) +from typing import Optional from copy import deepcopy from PathPlanning.TimeBasedPathPlanning.BaseClasses import MultiAgentPlanner, StartAndGoal from PathPlanning.TimeBasedPathPlanning.Node import NodePath @@ -33,7 +34,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c initial_solution: dict[AgentId, NodePath] = {} - # Generate initial solution (no reservations for robots) + # Generate initial solution (no constraints) for start_and_goal in start_and_goals: 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 @@ -44,126 +45,135 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c 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}") - if verbose: print(f"Expanding node with constraint {constraint_tree_node.constraint} and parent {constraint_tree_node.parent_idx}") + print(f"\tCOST: {constraint_tree_node.cost}") 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! + print(f"Found a path with constraints after {constraint_tree.expanded_node_count()} expansions:") + print(f"Final cost: {constraint_tree_node.cost}") return (start_and_goals, [constraint_tree_node.paths[start_and_goal.index] for start_and_goal in 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: - if verbose: - print(f"\nOuter loop step for agent {constrained_agent}") - 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) - num_expansions = constraint_tree.expanded_node_count() - if num_expansions % 50 == 0: - print(f"Expanded {num_expansions} nodes so far...") - print(f"\tlen of constraints {len(all_constraints)}") - - # Skip if we have already tried this set of constraints - constraint_hash = hash(frozenset(all_constraints)) - if constraint_hash in attempted_constraint_combos: - if verbose: - print(f"\tSkipping already attempted constraint combination: {all_constraints}") + new_path = ConflictBasedSearch.plan_for_agent(constrained_agent, all_constraints, constraint_tree, attempted_constraint_combos, grid, single_agent_planner_class, start_and_goals) + if not new_path: continue - 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) - - # Just plan for agent with new constraint - start_and_goal = ConflictBasedSearch.find_by_index(start_and_goals, constrained_agent.agent) - try: - 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(constraint_tree_node.paths) # TODO: not sure if deepcopy is actually needed + # Deepcopy to update with applied constraint and new paths + applied_constraint_parent = deepcopy(constraint_tree_node) + # TODO: could have a map under the hood to make these copies cheaper + paths: dict[AgentId, NodePath] = deepcopy(constraint_tree_node.paths) paths[constrained_agent.agent] = new_path - # for (agent_idx, path) in paths.items(): - # print(f"\nAgent {agent_idx} path:\n {path}") + if verbose: + for (agent_idx, path) in paths.items(): + print(f"\nAgent {agent_idx} path:\n {path}") applied_constraint_parent.constraint = applied_constraint + # applied_constraint_parent.paths = paths parent_idx = constraint_tree.add_expanded_node(applied_constraint_parent) 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(f"Found a path with constraints after {num_expansions} expansions:") + print(f"Found a path with constraints after {constraint_tree.expanded_node_count()} 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))]) + print(f"Final cost: {constraint_tree_node.cost}") return (start_and_goals, paths.values()) - # 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: bad function name - def find_by_index(start_and_goal_list: list[StartAndGoal], target_index: AgentId) -> AgentId: + def get_agents_start_and_goal(start_and_goal_list: list[StartAndGoal], target_index: AgentId) -> StartAndGoal: for item in start_and_goal_list: if item.index == target_index: return item raise RuntimeError(f"Could not find agent with index {target_index} in {start_and_goal_list}") + + def plan_for_agent(constrained_agent: ConstraintTreeNode, + all_constraints: list[AppliedConstraint], + constraint_tree: ConstraintTree, + attempted_constraint_combos: set, + grid: Grid, + single_agent_planner_class: SingleAgentPlanner, + start_and_goals: list[StartAndGoal]) -> Optional[tuple[list[StartAndGoal], list[NodePath]]]: + + num_expansions = constraint_tree.expanded_node_count() + if num_expansions % 50 == 0: + print(f"Expanded {num_expansions} nodes so far...") + print(f"\tNumber of constraints on expanded node: {len(all_constraints)}") + + # Skip if we have already tried this set of constraints + constraint_hash = hash(frozenset(all_constraints)) + if constraint_hash in attempted_constraint_combos: + if verbose: + print(f"\tSkipping already attempted constraint combination: {all_constraints}") + return None + 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) + + # Just plan for agent with new constraint + start_and_goal = ConflictBasedSearch.get_agents_start_and_goal(start_and_goals, constrained_agent.agent) + try: + 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) + return new_path + except Exception as e: + print(f"Error: {e}") + return None + # TODO -# * still discrepancies between sipp and A* # * fan out across multiple threads # * 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 np.random.seed(42) # For reproducibility 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(5)] # start_and_goals = [StartAndGoal(i, Position(1, 2*i), Position(19, 19-i)) for i in range(4)] # generate random start and goals - start_and_goals: list[StartAndGoal] = [] - for i in range(40): - start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - while any([start_and_goal.start == start for start_and_goal in start_and_goals]): - start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + # start_and_goals: list[StartAndGoal] = [] + # for i in range(40): + # start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + # goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + # while any([start_and_goal.start == start for start_and_goal in start_and_goals]): + # start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + # goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - start_and_goals.append(StartAndGoal(i, start, goal)) + # start_and_goals.append(StartAndGoal(i, start, goal)) # hallway cross # start_and_goals = [StartAndGoal(0, Position(6, 10), Position(13, 10)), @@ -183,8 +193,8 @@ def main(): obstacle_avoid_points=obstacle_avoid_points, # obstacle_arrangement=ObstacleArrangement.TEMPORARY_OBSTACLE, # obstacle_arrangement=ObstacleArrangement.HALLWAY, - # obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR, - obstacle_arrangement=ObstacleArrangement.NONE, + obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR, + # obstacle_arrangement=ObstacleArrangement.NONE, # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, # obstacle_arrangement=ObstacleArrangement.RANDOM, ) diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index af0f69447b..c266cc561c 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -150,9 +150,9 @@ 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 + node_idx = len(self.expanded_nodes) + self.expanded_nodes[node_idx] = node + return node_idx def expanded_node_count(self) -> int: return len(self.expanded_nodes) \ No newline at end of file From 0583d06a11e0051aa2ae28086df548227220ee7b Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Sat, 11 Oct 2025 19:37:50 -0400 Subject: [PATCH 11/29] constraint tree visualizer --- .../ConflictBasedSearch.py | 25 +- .../constraint_tree_viz.py | 216 ++++++++++++++++++ 2 files changed, 231 insertions(+), 10 deletions(-) create mode 100644 PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 0d4bd35d1a..de7e5aa1fc 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -21,10 +21,14 @@ from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId, AppliedConstraint, ConstraintTree, ConstraintTreeNode, ForkingConstraint import time +# TODO: dont include this +from constraint_tree_viz import visualize_cbs_tree class ConflictBasedSearch(MultiAgentPlanner): + + # TODO: remove ConstraintTree from return @staticmethod - def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]: + def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath], ConstraintTree]: """ Generate a path from the start to the goal for each agent in the `start_and_goals` list. Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans @@ -97,7 +101,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c for constraint in all_constraints: print(f"\t{constraint}") print(f"Final cost: {constraint_tree_node.cost}") - return (start_and_goals, paths.values()) + return (start_and_goals, paths.values(), constraint_tree) if verbose: print(f"Adding new constraint tree node with constraint: {new_constraint_tree_node.constraint}") @@ -161,7 +165,7 @@ def main(): grid_side_length = 21 # 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(5)] + # start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(5)] # start_and_goals = [StartAndGoal(i, Position(1, 2*i), Position(19, 19-i)) for i in range(4)] # generate random start and goals @@ -176,9 +180,9 @@ def main(): # start_and_goals.append(StartAndGoal(i, start, goal)) # 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))] @@ -192,15 +196,15 @@ 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.NONE, # 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, constraint_tree = 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 @@ -214,7 +218,8 @@ def main(): if not show_animation: return - PlotNodePaths(grid, start_and_goals, paths) + visualize_cbs_tree(constraint_tree.expanded_nodes, constraint_tree.nodes_to_expand) + # PlotNodePaths(grid, start_and_goals, paths) if __name__ == "__main__": main() \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py b/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py new file mode 100644 index 0000000000..4b3bef9b58 --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py @@ -0,0 +1,216 @@ +import plotly.graph_objects as go +import plotly.express as px +from plotly.subplots import make_subplots +import networkx as nx +from typing import Optional, Dict, Any +from dataclasses import dataclass +from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId, AppliedConstraint, ConstraintTree, ConstraintTreeNode, ForkingConstraint + +def visualize_cbs_tree( + expanded_nodes: Dict[int, ConstraintTreeNode], + nodes_to_expand: list[ConstraintTreeNode], + initial_size: int = 15 +) -> None: + """ + Visualize the CBS constraint tree with interactive nodes. + Click a node to print its details to console. + """ + + # Build networkx graph + G = nx.DiGraph() + + # Add all nodes with metadata + node_colors = [] + node_sizes = [] + node_labels = {} + + for idx, node in expanded_nodes.items(): + G.add_node(idx) + node_labels[idx] = f"Node {idx}
Cost: {node.cost}
Parent: {node.parent_idx}
Constraint:
{node.constraint}" + node_colors.append("lightblue") + node_sizes.append(initial_size) + + # Add edge from parent + # if node.parent_idx is not None: + if node.parent_idx is not None and node.parent_idx in expanded_nodes: + G.add_edge(node.parent_idx, idx) + # G.add_edge(0, 5) + print(f"adding edge btwn {node.parent_idx} and {idx}") + + # Add unexpanded nodes + # unexpanded_node_map = {} + # for node in nodes_to_expand: + # idx = id(node) # Use object id for heap nodes + # if idx not in G.nodes(): + # G.add_node(idx) + # # node_labels[idx] = f"Node {idx}\n(cost: {node.cost})" + # node_labels[idx] = f"Node {idx}
Cost: {node.cost}
Constraint:
{node.constraint}" + # node_colors.append("lightyellow") + # node_sizes.append(initial_size) + # unexpanded_node_map[idx] = node + + # if node.parent_idx is not None and node.parent_idx >= 0: + # G.add_edge(node.parent_idx, idx) + + # Use hierarchical layout with fixed horizontal spacing + pos = _hierarchy_pos(G, root=next(iter(G.nodes()), None), vert_gap=0.3, horiz_gap=1.5) + + # Extract edge coordinates + edge_x = [] + edge_y = [] + for edge in G.edges(): + print(f"Drawing edge: {edge}") + if edge[0] in pos and edge[1] in pos: + x0, y0 = pos[edge[0]] + x1, y1 = pos[edge[1]] + edge_x.extend([x0, x1, None]) + edge_y.extend([y0, y1, None]) + else: + edge_x.extend([1, 1, None]) + edge_y.extend([5, 5, None]) + + # Extract node coordinates + node_x = [] + node_y = [] + for node in G.nodes(): + x, y = 1, 1 + if node in pos: + x, y = pos[node] + node_x.append(x) + node_y.append(y) + + # Create figure + fig = go.Figure() + + # Add edges + fig.add_trace(go.Scatter( + x=edge_x, y=edge_y, + mode='lines', + line=dict(width=2, color='#888'), + hoverinfo='none', + showlegend=False + )) + # Add nodes + fig.add_trace(go.Scatter( + x=node_x, y=node_y, + mode='markers', + marker=dict( + size=node_sizes, + color=node_colors, + line=dict(width=2, color='darkblue') + ), + text=[node_labels[node] for node in G.nodes() if node in node_labels], + hoverinfo='text', + showlegend=False, + customdata=list(G.nodes()) + )) + + fig.update_layout( + title="CBS Constraint Tree", + showlegend=False, + hovermode='closest', + margin=dict(b=20, l=5, r=5, t=40), + xaxis=dict( + showgrid=False, + zeroline=False, + showticklabels=False, + scaleanchor="y", + scaleratio=1 + ), + yaxis=dict( + showgrid=False, + zeroline=False, + showticklabels=False, + scaleanchor="x", + scaleratio=1 + ), + plot_bgcolor='white', + autosize=True, + ) + + # Add click event + fig.update_traces( + selector=dict(mode='markers'), + customdata=list(G.nodes()), + hovertemplate='%{text}' + ) + + fig.update_xaxes(fixedrange=False) + fig.update_yaxes(fixedrange=False) + + # Show and set up click handler + fig.show() + + # Print handler instructions + print("\nCBS Tree Visualization") + print("=" * 50) + print("Hover over nodes to see cost") + print("Right-click → 'Inspect' → Open browser console") + print("Then paste this to get node info:\n") + print("for (let node of document.querySelectorAll('circle')) {") + print(" node.onclick = (e) => {") + print(" console.log('Clicked node:', e.target);") + print(" }") + print("}\n") + print("Or use the alternative: Print all nodes programmatically:\n") + +def _hierarchy_pos(G, root=None, vert_gap=0.2, horiz_gap=1.0, xcenter=0.5): + """ + Create hierarchical layout for tree visualization with fixed horizontal spacing. + """ + if not nx.is_tree(G): + G = nx.DiGraph(G) + + def _hierarchy_pos_recursive(G, root, vert_gap=0.2, horiz_gap=1.0, xcenter=0.5, pos=None, parent=None, child_index=0): + if pos is None: + pos = {root: (xcenter, 0)} + else: + pos[root] = (xcenter, pos[parent][1] - vert_gap) + + neighbors = list(G.neighbors(root)) + + if len(neighbors) != 0: + num_children = len(neighbors) + # Spread children horizontally with fixed gap + start_x = xcenter - (num_children - 1) * horiz_gap / 2 + for i, neighbor in enumerate(neighbors): + nextx = start_x + i * horiz_gap + _hierarchy_pos_recursive(G, neighbor, vert_gap=vert_gap, horiz_gap=horiz_gap, + xcenter=nextx, pos=pos, parent=root, child_index=i) + + return pos + + return _hierarchy_pos_recursive(G, root, vert_gap, horiz_gap, xcenter) + + +# Example usage: +if __name__ == "__main__": + from dataclasses import dataclass + from typing import Optional + + @dataclass + class MockConstraint: + agent: int + time: int + location: tuple + + def __repr__(self): + return f"Constraint(agent={self.agent}, t={self.time}, loc={self.location})" + + @dataclass + class MockNode: + parent_idx: Optional[int] + constraint: Optional[MockConstraint] + paths: dict + cost: int + + # Create mock tree + nodes = { + 0: MockNode(None, None, {"a": [], "b": []}, 10), + 1: MockNode(0, MockConstraint(0, 2, (0, 0)), {"a": [(0,0), (1,0)], "b": [(0,1), (0,2)]}, 12), + 2: MockNode(0, MockConstraint(1, 1, (0,1)), {"a": [(0,0), (1,0)], "b": [(0,1), (0,2)]}, 11), + 3: MockNode(1, MockConstraint(0, 3, (1,0)), {"a": [(0,0), (1,0), (1,1)], "b": [(0,1), (0,2)]}, 14), + 4: MockNode(2, None, {"a": [(0,0), (1,0)], "b": [(0,1), (1,1)]}, 12), + } + + visualize_cbs_tree(nodes, []) \ No newline at end of file From 4d380199c0dd73eb9e767effebb49f4067d86cf6 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Sat, 11 Oct 2025 19:46:29 -0400 Subject: [PATCH 12/29] idk --- .../constraint_tree_viz.py | 77 +++++++++++++++---- 1 file changed, 62 insertions(+), 15 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py b/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py index 4b3bef9b58..f3b4bea85b 100644 --- a/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py +++ b/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py @@ -30,11 +30,9 @@ def visualize_cbs_tree( node_colors.append("lightblue") node_sizes.append(initial_size) - # Add edge from parent - # if node.parent_idx is not None: + # Add edge from parent only if parent exists in expanded_nodes if node.parent_idx is not None and node.parent_idx in expanded_nodes: G.add_edge(node.parent_idx, idx) - # G.add_edge(0, 5) print(f"adding edge btwn {node.parent_idx} and {idx}") # Add unexpanded nodes @@ -52,37 +50,68 @@ def visualize_cbs_tree( # if node.parent_idx is not None and node.parent_idx >= 0: # G.add_edge(node.parent_idx, idx) + # Debug: print all edges in the graph + print(f"\nAll edges in graph: {list(G.edges())}") + print(f"All nodes in graph: {list(G.nodes())}") + + # Use hierarchical layout with fixed horizontal spacing - pos = _hierarchy_pos(G, root=next(iter(G.nodes()), None), vert_gap=0.3, horiz_gap=1.5) + # Handle disconnected components + pos = {} + x_offset = 0 + for component in nx.weakly_connected_components(G): + subgraph = G.subgraph(component) + root = next(iter(subgraph.nodes())) + component_pos = _hierarchy_pos(subgraph, root=root, vert_gap=0.3, horiz_gap=3.0) + + # Offset each component horizontally to avoid overlap + for node, (x, y) in component_pos.items(): + pos[node] = (x + x_offset, y) + + # Increment x_offset for next component + if len(component_pos) > 0: + x_offset += max(x for x, y in component_pos.values()) + 3 - # Extract edge coordinates + print(f"Positions: {pos}") + + # Extract edge coordinates with hover text edge_x = [] edge_y = [] + edge_text = [] for edge in G.edges(): print(f"Drawing edge: {edge}") if edge[0] in pos and edge[1] in pos: x0, y0 = pos[edge[0]] x1, y1 = pos[edge[1]] + print(f" From ({x0}, {y0}) to ({x1}, {y1})") edge_x.extend([x0, x1, None]) edge_y.extend([y0, y1, None]) + edge_text.extend([f"Node {edge[0]} → Node {edge[1]}", f"Node {edge[0]} → Node {edge[1]}", None]) else: + print(f" Edge position not found") edge_x.extend([1, 1, None]) edge_y.extend([5, 5, None]) - + edge_text.extend([f"Node {edge[0]} → Node {edge[1]}", f"Node {edge[0]} → Node {edge[1]}", None]) + # Extract node coordinates node_x = [] node_y = [] - for node in G.nodes(): + node_list = list(G.nodes()) + for node in node_list: x, y = 1, 1 if node in pos: x, y = pos[node] + else: + print(f"WARNING: Node {node} not in positions!") node_x.append(x) node_y.append(y) + print(f"Node coordinates: {list(zip(node_list, node_x, node_y))}") + # Create figure fig = go.Figure() - # Add edges + # Add edges (visible lines) fig.add_trace(go.Scatter( x=edge_x, y=edge_y, mode='lines', @@ -90,6 +119,17 @@ def visualize_cbs_tree( hoverinfo='none', showlegend=False )) + + # Add invisible thick edges for hover detection + fig.add_trace(go.Scatter( + x=edge_x, y=edge_y, + mode='lines', + line=dict(width=20, color='rgba(0,0,0,0)'), + text=edge_text, + hoverinfo='text', + showlegend=False + )) + # Add nodes fig.add_trace(go.Scatter( x=node_x, y=node_y, @@ -99,10 +139,10 @@ def visualize_cbs_tree( color=node_colors, line=dict(width=2, color='darkblue') ), - text=[node_labels[node] for node in G.nodes() if node in node_labels], + text=[node_labels.get(node, f"Node {node}") for node in node_list], hoverinfo='text', showlegend=False, - customdata=list(G.nodes()) + customdata=node_list )) fig.update_layout( @@ -157,26 +197,33 @@ def visualize_cbs_tree( def _hierarchy_pos(G, root=None, vert_gap=0.2, horiz_gap=1.0, xcenter=0.5): """ Create hierarchical layout for tree visualization with fixed horizontal spacing. + Spreads nodes wide at each level to avoid overlaps. """ if not nx.is_tree(G): G = nx.DiGraph(G) - def _hierarchy_pos_recursive(G, root, vert_gap=0.2, horiz_gap=1.0, xcenter=0.5, pos=None, parent=None, child_index=0): + def _hierarchy_pos_recursive(G, root, vert_gap=0.2, horiz_gap=1.0, xcenter=0.5, pos=None, parent=None, child_index=0, level_nodes=None): if pos is None: pos = {root: (xcenter, 0)} + level_nodes = {0: [root]} else: + level = pos[parent][1] / (-vert_gap) pos[root] = (xcenter, pos[parent][1] - vert_gap) + if int(level) + 1 not in level_nodes: + level_nodes[int(level) + 1] = [] + level_nodes[int(level) + 1].append(root) neighbors = list(G.neighbors(root)) if len(neighbors) != 0: num_children = len(neighbors) - # Spread children horizontally with fixed gap - start_x = xcenter - (num_children - 1) * horiz_gap / 2 + # Spread children very wide to avoid any overlap + spread = num_children * horiz_gap * 2 + start_x = xcenter - spread / 2 for i, neighbor in enumerate(neighbors): - nextx = start_x + i * horiz_gap + nextx = start_x + i * (spread / max(num_children - 1, 1)) _hierarchy_pos_recursive(G, neighbor, vert_gap=vert_gap, horiz_gap=horiz_gap, - xcenter=nextx, pos=pos, parent=root, child_index=i) + xcenter=nextx, pos=pos, parent=root, child_index=i, level_nodes=level_nodes) return pos From 3ffd2ffa173ca38f6bfd57868269bcaae49e1c0f Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Sun, 12 Oct 2025 10:48:30 -0400 Subject: [PATCH 13/29] testing - on par with priority in empty area --- .../ConflictBasedSearch.py | 38 ++- .../constraint_tree_viz.py | 263 ------------------ 2 files changed, 17 insertions(+), 284 deletions(-) delete mode 100644 PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index de7e5aa1fc..41716ddf45 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -21,14 +21,11 @@ from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId, AppliedConstraint, ConstraintTree, ConstraintTreeNode, ForkingConstraint import time -# TODO: dont include this -from constraint_tree_viz import visualize_cbs_tree class ConflictBasedSearch(MultiAgentPlanner): - # TODO: remove ConstraintTree from return @staticmethod - def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath], ConstraintTree]: + def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]: """ Generate a path from the start to the goal for each agent in the `start_and_goals` list. Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans @@ -101,7 +98,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c for constraint in all_constraints: print(f"\t{constraint}") print(f"Final cost: {constraint_tree_node.cost}") - return (start_and_goals, paths.values(), constraint_tree) + return (start_and_goals, paths.values()) if verbose: print(f"Adding new constraint tree node with constraint: {new_constraint_tree_node.constraint}") @@ -169,20 +166,20 @@ def main(): # start_and_goals = [StartAndGoal(i, Position(1, 2*i), Position(19, 19-i)) for i in range(4)] # generate random start and goals - # start_and_goals: list[StartAndGoal] = [] - # for i in range(40): - # start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - # goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - # while any([start_and_goal.start == start for start_and_goal in start_and_goals]): - # start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - # goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + start_and_goals: list[StartAndGoal] = [] + for i in range(40): + start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + while any([start_and_goal.start == start for start_and_goal in start_and_goals]): + start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) + goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - # start_and_goals.append(StartAndGoal(i, start, goal)) + start_and_goals.append(StartAndGoal(i, start, goal)) # 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))] @@ -196,15 +193,15 @@ def main(): num_obstacles=250, obstacle_avoid_points=obstacle_avoid_points, # obstacle_arrangement=ObstacleArrangement.TEMPORARY_OBSTACLE, - obstacle_arrangement=ObstacleArrangement.HALLWAY, + # obstacle_arrangement=ObstacleArrangement.HALLWAY, # obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR, - # obstacle_arrangement=ObstacleArrangement.NONE, + obstacle_arrangement=ObstacleArrangement.NONE, # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, # obstacle_arrangement=ObstacleArrangement.RANDOM, ) start_time = time.time() - start_and_goals, paths, constraint_tree = ConflictBasedSearch.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) + 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 @@ -218,8 +215,7 @@ def main(): if not show_animation: return - visualize_cbs_tree(constraint_tree.expanded_nodes, constraint_tree.nodes_to_expand) - # PlotNodePaths(grid, start_and_goals, paths) + PlotNodePaths(grid, start_and_goals, paths) if __name__ == "__main__": main() \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py b/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py deleted file mode 100644 index f3b4bea85b..0000000000 --- a/PathPlanning/TimeBasedPathPlanning/constraint_tree_viz.py +++ /dev/null @@ -1,263 +0,0 @@ -import plotly.graph_objects as go -import plotly.express as px -from plotly.subplots import make_subplots -import networkx as nx -from typing import Optional, Dict, Any -from dataclasses import dataclass -from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId, AppliedConstraint, ConstraintTree, ConstraintTreeNode, ForkingConstraint - -def visualize_cbs_tree( - expanded_nodes: Dict[int, ConstraintTreeNode], - nodes_to_expand: list[ConstraintTreeNode], - initial_size: int = 15 -) -> None: - """ - Visualize the CBS constraint tree with interactive nodes. - Click a node to print its details to console. - """ - - # Build networkx graph - G = nx.DiGraph() - - # Add all nodes with metadata - node_colors = [] - node_sizes = [] - node_labels = {} - - for idx, node in expanded_nodes.items(): - G.add_node(idx) - node_labels[idx] = f"Node {idx}
Cost: {node.cost}
Parent: {node.parent_idx}
Constraint:
{node.constraint}" - node_colors.append("lightblue") - node_sizes.append(initial_size) - - # Add edge from parent only if parent exists in expanded_nodes - if node.parent_idx is not None and node.parent_idx in expanded_nodes: - G.add_edge(node.parent_idx, idx) - print(f"adding edge btwn {node.parent_idx} and {idx}") - - # Add unexpanded nodes - # unexpanded_node_map = {} - # for node in nodes_to_expand: - # idx = id(node) # Use object id for heap nodes - # if idx not in G.nodes(): - # G.add_node(idx) - # # node_labels[idx] = f"Node {idx}\n(cost: {node.cost})" - # node_labels[idx] = f"Node {idx}
Cost: {node.cost}
Constraint:
{node.constraint}" - # node_colors.append("lightyellow") - # node_sizes.append(initial_size) - # unexpanded_node_map[idx] = node - - # if node.parent_idx is not None and node.parent_idx >= 0: - # G.add_edge(node.parent_idx, idx) - - # Debug: print all edges in the graph - print(f"\nAll edges in graph: {list(G.edges())}") - print(f"All nodes in graph: {list(G.nodes())}") - - - # Use hierarchical layout with fixed horizontal spacing - # Handle disconnected components - pos = {} - x_offset = 0 - for component in nx.weakly_connected_components(G): - subgraph = G.subgraph(component) - root = next(iter(subgraph.nodes())) - component_pos = _hierarchy_pos(subgraph, root=root, vert_gap=0.3, horiz_gap=3.0) - - # Offset each component horizontally to avoid overlap - for node, (x, y) in component_pos.items(): - pos[node] = (x + x_offset, y) - - # Increment x_offset for next component - if len(component_pos) > 0: - x_offset += max(x for x, y in component_pos.values()) + 3 - - print(f"Positions: {pos}") - - # Extract edge coordinates with hover text - edge_x = [] - edge_y = [] - edge_text = [] - for edge in G.edges(): - print(f"Drawing edge: {edge}") - if edge[0] in pos and edge[1] in pos: - x0, y0 = pos[edge[0]] - x1, y1 = pos[edge[1]] - print(f" From ({x0}, {y0}) to ({x1}, {y1})") - edge_x.extend([x0, x1, None]) - edge_y.extend([y0, y1, None]) - edge_text.extend([f"Node {edge[0]} → Node {edge[1]}", f"Node {edge[0]} → Node {edge[1]}", None]) - else: - print(f" Edge position not found") - edge_x.extend([1, 1, None]) - edge_y.extend([5, 5, None]) - edge_text.extend([f"Node {edge[0]} → Node {edge[1]}", f"Node {edge[0]} → Node {edge[1]}", None]) - - # Extract node coordinates - node_x = [] - node_y = [] - node_list = list(G.nodes()) - for node in node_list: - x, y = 1, 1 - if node in pos: - x, y = pos[node] - else: - print(f"WARNING: Node {node} not in positions!") - node_x.append(x) - node_y.append(y) - - print(f"Node coordinates: {list(zip(node_list, node_x, node_y))}") - - # Create figure - fig = go.Figure() - - # Add edges (visible lines) - fig.add_trace(go.Scatter( - x=edge_x, y=edge_y, - mode='lines', - line=dict(width=2, color='#888'), - hoverinfo='none', - showlegend=False - )) - - # Add invisible thick edges for hover detection - fig.add_trace(go.Scatter( - x=edge_x, y=edge_y, - mode='lines', - line=dict(width=20, color='rgba(0,0,0,0)'), - text=edge_text, - hoverinfo='text', - showlegend=False - )) - - # Add nodes - fig.add_trace(go.Scatter( - x=node_x, y=node_y, - mode='markers', - marker=dict( - size=node_sizes, - color=node_colors, - line=dict(width=2, color='darkblue') - ), - text=[node_labels.get(node, f"Node {node}") for node in node_list], - hoverinfo='text', - showlegend=False, - customdata=node_list - )) - - fig.update_layout( - title="CBS Constraint Tree", - showlegend=False, - hovermode='closest', - margin=dict(b=20, l=5, r=5, t=40), - xaxis=dict( - showgrid=False, - zeroline=False, - showticklabels=False, - scaleanchor="y", - scaleratio=1 - ), - yaxis=dict( - showgrid=False, - zeroline=False, - showticklabels=False, - scaleanchor="x", - scaleratio=1 - ), - plot_bgcolor='white', - autosize=True, - ) - - # Add click event - fig.update_traces( - selector=dict(mode='markers'), - customdata=list(G.nodes()), - hovertemplate='%{text}' - ) - - fig.update_xaxes(fixedrange=False) - fig.update_yaxes(fixedrange=False) - - # Show and set up click handler - fig.show() - - # Print handler instructions - print("\nCBS Tree Visualization") - print("=" * 50) - print("Hover over nodes to see cost") - print("Right-click → 'Inspect' → Open browser console") - print("Then paste this to get node info:\n") - print("for (let node of document.querySelectorAll('circle')) {") - print(" node.onclick = (e) => {") - print(" console.log('Clicked node:', e.target);") - print(" }") - print("}\n") - print("Or use the alternative: Print all nodes programmatically:\n") - -def _hierarchy_pos(G, root=None, vert_gap=0.2, horiz_gap=1.0, xcenter=0.5): - """ - Create hierarchical layout for tree visualization with fixed horizontal spacing. - Spreads nodes wide at each level to avoid overlaps. - """ - if not nx.is_tree(G): - G = nx.DiGraph(G) - - def _hierarchy_pos_recursive(G, root, vert_gap=0.2, horiz_gap=1.0, xcenter=0.5, pos=None, parent=None, child_index=0, level_nodes=None): - if pos is None: - pos = {root: (xcenter, 0)} - level_nodes = {0: [root]} - else: - level = pos[parent][1] / (-vert_gap) - pos[root] = (xcenter, pos[parent][1] - vert_gap) - if int(level) + 1 not in level_nodes: - level_nodes[int(level) + 1] = [] - level_nodes[int(level) + 1].append(root) - - neighbors = list(G.neighbors(root)) - - if len(neighbors) != 0: - num_children = len(neighbors) - # Spread children very wide to avoid any overlap - spread = num_children * horiz_gap * 2 - start_x = xcenter - spread / 2 - for i, neighbor in enumerate(neighbors): - nextx = start_x + i * (spread / max(num_children - 1, 1)) - _hierarchy_pos_recursive(G, neighbor, vert_gap=vert_gap, horiz_gap=horiz_gap, - xcenter=nextx, pos=pos, parent=root, child_index=i, level_nodes=level_nodes) - - return pos - - return _hierarchy_pos_recursive(G, root, vert_gap, horiz_gap, xcenter) - - -# Example usage: -if __name__ == "__main__": - from dataclasses import dataclass - from typing import Optional - - @dataclass - class MockConstraint: - agent: int - time: int - location: tuple - - def __repr__(self): - return f"Constraint(agent={self.agent}, t={self.time}, loc={self.location})" - - @dataclass - class MockNode: - parent_idx: Optional[int] - constraint: Optional[MockConstraint] - paths: dict - cost: int - - # Create mock tree - nodes = { - 0: MockNode(None, None, {"a": [], "b": []}, 10), - 1: MockNode(0, MockConstraint(0, 2, (0, 0)), {"a": [(0,0), (1,0)], "b": [(0,1), (0,2)]}, 12), - 2: MockNode(0, MockConstraint(1, 1, (0,1)), {"a": [(0,0), (1,0)], "b": [(0,1), (0,2)]}, 11), - 3: MockNode(1, MockConstraint(0, 3, (1,0)), {"a": [(0,0), (1,0), (1,1)], "b": [(0,1), (0,2)]}, 14), - 4: MockNode(2, None, {"a": [(0,0), (1,0)], "b": [(0,1), (1,1)]}, 12), - } - - visualize_cbs_tree(nodes, []) \ No newline at end of file From 2d3cdde5c31055b4b402c160ceba2a491db6616c Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 13:14:18 -0500 Subject: [PATCH 14/29] agent id todo --- PathPlanning/TimeBasedPathPlanning/BaseClasses.py | 4 ++-- .../TimeBasedPathPlanning/ConflictBasedSearch.py | 12 ++++++------ .../GridWithDynamicObstacles.py | 4 ++-- PathPlanning/TimeBasedPathPlanning/Plotting.py | 4 ++-- .../TimeBasedPathPlanning/PriorityBasedPlanner.py | 6 +++--- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 6f64fdd83b..6c7b95a747 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -4,6 +4,7 @@ Grid, Position, ) +from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId from PathPlanning.TimeBasedPathPlanning.Node import NodePath import random import numpy.random as numpy_random @@ -26,8 +27,7 @@ def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: b @dataclass class StartAndGoal: # Index of this agent - # TODO: better name and use AgentId type - index: int + agent_id: AgentId # Start position of the robot start: Position # Goal position of the robot diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 41716ddf45..b6d043019c 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -37,8 +37,8 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c # Generate initial solution (no constraints) for start_and_goal in start_and_goals: - 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 + path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose) + initial_solution[AgentId(start_and_goal.agent_id)] = path if verbose: print("Initial solution:") @@ -62,7 +62,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c # This means we found a solution! print(f"Found a path with constraints after {constraint_tree.expanded_node_count()} expansions:") print(f"Final cost: {constraint_tree_node.cost}") - return (start_and_goals, [constraint_tree_node.paths[start_and_goal.index] for start_and_goal in start_and_goals]) + return (start_and_goals, [constraint_tree_node.paths[start_and_goal.agent_id] for start_and_goal in start_and_goals]) if not isinstance(constraint_tree_node.constraint, ForkingConstraint): raise ValueError(f"Expected a ForkingConstraint, but got: {constraint_tree_node.constraint}") @@ -108,7 +108,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c def get_agents_start_and_goal(start_and_goal_list: list[StartAndGoal], target_index: AgentId) -> StartAndGoal: for item in start_and_goal_list: - if item.index == target_index: + if item.agent_id == target_index: return item raise RuntimeError(f"Could not find agent with index {target_index} in {start_and_goal_list}") @@ -142,11 +142,11 @@ def plan_for_agent(constrained_agent: ConstraintTreeNode, grid.apply_constraint_points(all_constraints) # Just plan for agent with new constraint - start_and_goal = ConflictBasedSearch.get_agents_start_and_goal(start_and_goals, constrained_agent.agent) + start_and_goal: StartAndGoal = ConflictBasedSearch.get_agents_start_and_goal(start_and_goals, constrained_agent.agent) try: 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) + new_path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose) return new_path except Exception as e: print(f"Error: {e}") diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 285a418fd0..1f8323f7dc 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -3,9 +3,9 @@ is also infrastructure to generate dynamic obstacles that move around the grid. The obstacles' paths are stored in the reservation matrix on creation. """ -import numpy as np -import matplotlib.pyplot as plt from enum import Enum +import matplotlib.pyplot as plt +import numpy as np from dataclasses import dataclass from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AppliedConstraint diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index 7cd1f615d8..a7de25e4b2 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -71,7 +71,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N start_and_goal_plots = [] for i, path in enumerate(paths): marker_idx = i % len(markers) - agent_id = start_and_goals[i].index + agent_id = start_and_goals[i].agent_id start = start_and_goals[i].start goal = start_and_goals[i].goal @@ -88,7 +88,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N # Create plots for each agent's path path_plots = [] for i, path in enumerate(paths): - agent_id = start_and_goals[i].index + agent_id = start_and_goals[i].agent_id path_plot, = ax.plot([], [], "o", c=colors[i], ms=10, label=f"Agent {agent_id} Path") path_plots.append(path_plot) diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index 2573965cf6..f297992867 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -33,7 +33,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c # 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)) + grid.reserve_position(start_and_goal.start, start_and_goal.agent_id, Interval(0, 10)) # Plan in descending order of distance from start to goal start_and_goals = sorted(start_and_goals, @@ -45,14 +45,14 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c if verbose: print(f"\nPlanning for agent: {start_and_goal}" ) - grid.clear_initial_reservation(start_and_goal.start, start_and_goal.index) + grid.clear_initial_reservation(start_and_goal.start, start_and_goal.agent_id) path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, verbose) if path is None: print(f"Failed to find path for {start_and_goal}") return [] - agent_index = start_and_goal.index + agent_index = start_and_goal.agent_id grid.reserve_path(path, agent_index) paths.append(path) From d822aa778a668e66fde95bfbd0802ccf63ead196 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 13:54:37 -0500 Subject: [PATCH 15/29] docstring, cleanup, unit tests --- .../TimeBasedPathPlanning/BaseClasses.py | 2 +- .../ConflictBasedSearch.py | 103 ++++++++---------- .../TimeBasedPathPlanning/ConstraintTree.py | 25 ++--- tests/test_conflict_based_search.py | 92 ++++++++++++++++ 4 files changed, 150 insertions(+), 72 deletions(-) create mode 100644 tests/test_conflict_based_search.py diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 6c7b95a747..0e304efeac 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -21,7 +21,7 @@ class SingleAgentPlanner(ABC): @staticmethod @abstractmethod - def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: bool = False) -> NodePath: + def plan(grid: Grid, start: Position, goal: Position, agent_idx: AgentId, verbose: bool = False) -> NodePath: pass @dataclass diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index b6d043019c..610701ac48 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -1,9 +1,19 @@ """ -TODO - docstring - -Algorithm is defined in this paper: https://cdn.aaai.org/ojs/8140/8140-13-11667-1-2-20201228.pdf +Conflict Based Search generates paths in 2 dimensions (x, y, time) for a set of agents. It does +so by performing searches on two levels. The top level search applies constraints that agents +must avoid, and the bottom level performs a single-agent search for individual agents given +the constraints provided by the top level search. Initially, paths are generated for each agent +with no constraints. The paths are checked for conflicts with one another. If any are found, the +top level search generates two nodes. These nodes apply a constraint at the point of conflict for +one of the two agents in conflict. This process repeats until a set of paths are found where no +agents are in conflict. The top level search chooses successor nodes based on the sum of the +cost of all paths, which guarantees optimiality of the final solution. + +The full algorithm is defined in this paper: https://cdn.aaai.org/ojs/8140/8140-13-11667-1-2-20201228.pdf """ +from copy import deepcopy +from enum import Enum import numpy as np from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( Grid, @@ -11,7 +21,6 @@ Position, ) from typing import Optional -from copy import deepcopy from PathPlanning.TimeBasedPathPlanning.BaseClasses import MultiAgentPlanner, StartAndGoal from PathPlanning.TimeBasedPathPlanning.Node import NodePath from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner @@ -60,8 +69,9 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c raise RuntimeError("No more nodes to expand in the constraint tree.") if not constraint_tree_node.constraint: # This means we found a solution! - print(f"Found a path with constraints after {constraint_tree.expanded_node_count()} expansions:") + print(f"\nFound a path with constraints after {constraint_tree.expanded_node_count()} expansions") print(f"Final cost: {constraint_tree_node.cost}") + print(f"Number of constraints on solution: {len(constraint_tree_node.all_constraints)}") return (start_and_goals, [constraint_tree_node.paths[start_and_goal.agent_id] for start_and_goal in start_and_goals]) if not isinstance(constraint_tree_node.constraint, ForkingConstraint): @@ -70,36 +80,26 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c for constrained_agent in constraint_tree_node.constraint.constrained_agents: applied_constraint = AppliedConstraint(constrained_agent.constraint, constrained_agent.agent) - all_constraints = deepcopy(ancestor_constraints) # TODO - no deepcopy pls + all_constraints = ancestor_constraints all_constraints.append(applied_constraint) - new_path = ConflictBasedSearch.plan_for_agent(constrained_agent, all_constraints, constraint_tree, attempted_constraint_combos, grid, single_agent_planner_class, start_and_goals) + new_path = ConflictBasedSearch.plan_for_agent(constrained_agent, all_constraints, constraint_tree, attempted_constraint_combos, grid, single_agent_planner_class, start_and_goals, verbose) if not new_path: continue # Deepcopy to update with applied constraint and new paths applied_constraint_parent = deepcopy(constraint_tree_node) - # TODO: could have a map under the hood to make these copies cheaper - paths: dict[AgentId, NodePath] = deepcopy(constraint_tree_node.paths) - paths[constrained_agent.agent] = new_path + # Copy paths for child node - we just need to update constrained agent's path + applied_constraint_parent.paths[constrained_agent.agent] = new_path if verbose: - for (agent_idx, path) in paths.items(): + for (agent_idx, path) in applied_constraint_parent.paths.items(): print(f"\nAgent {agent_idx} path:\n {path}") applied_constraint_parent.constraint = applied_constraint - # applied_constraint_parent.paths = paths parent_idx = constraint_tree.add_expanded_node(applied_constraint_parent) - 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(f"Found a path with constraints after {constraint_tree.expanded_node_count()} expansions:") - for constraint in all_constraints: - print(f"\t{constraint}") - print(f"Final cost: {constraint_tree_node.cost}") - return (start_and_goals, paths.values()) - + new_constraint_tree_node = ConstraintTreeNode(applied_constraint_parent.paths, parent_idx, all_constraints) 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) @@ -119,7 +119,8 @@ def plan_for_agent(constrained_agent: ConstraintTreeNode, attempted_constraint_combos: set, grid: Grid, single_agent_planner_class: SingleAgentPlanner, - start_and_goals: list[StartAndGoal]) -> Optional[tuple[list[StartAndGoal], list[NodePath]]]: + start_and_goals: list[StartAndGoal], + verbose: False) -> Optional[tuple[list[StartAndGoal], list[NodePath]]]: num_expansions = constraint_tree.expanded_node_count() if num_expansions % 50 == 0: @@ -149,55 +150,41 @@ def plan_for_agent(constrained_agent: ConstraintTreeNode, new_path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose) return new_path except Exception as e: - print(f"Error: {e}") + if verbose: + print(f"Error: {e}") return None -# TODO -# * fan out across multiple threads -# * somehow test/check that high level tree is doing what you want +class Scenario(Enum): + # Five robots all trying to get through a single cell choke point to reach their goals + NARROW_CORRIDOR = 0 + # Three robots in a narrow hallway that requires intelligent conflict resolution + HALLWAY_CROSS = 1 + +scenario = Scenario.HALLWAY_CROSS verbose = False show_animation = True np.random.seed(42) # For reproducibility def main(): grid_side_length = 21 - # 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(5)] - # start_and_goals = [StartAndGoal(i, Position(1, 2*i), Position(19, 19-i)) for i in range(4)] - - # generate random start and goals - start_and_goals: list[StartAndGoal] = [] - for i in range(40): - start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - while any([start_and_goal.start == start for start_and_goal in start_and_goals]): - start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length)) - - start_and_goals.append(StartAndGoal(i, start, goal)) - - # 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))] - - # temporary obstacle - # start_and_goals = [StartAndGoal(0, Position(15, 14), Position(15, 16))] + # Default: no obstacles + obstacle_arrangement = ObstacleArrangement.NONE + start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(10)] - # 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)] + if scenario == Scenario.NARROW_CORRIDOR: + obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR + start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(5)] + elif scenario == Scenario.HALLWAY_CROSS: + obstacle_arrangement=ObstacleArrangement.HALLWAY + 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))] 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.NONE, - # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, - # obstacle_arrangement=ObstacleArrangement.RANDOM, + obstacle_avoid_points=[pos for item in start_and_goals for pos in (item.start, item.goal)], + obstacle_arrangement=obstacle_arrangement, ) start_time = time.time() diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index c266cc561c..d7eb7a12f2 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -50,7 +50,6 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]: 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 possible_constraints: list[ForkingConstraint] = [] for agent_id, path in self.paths.items(): # Check for edge conflicts @@ -61,7 +60,6 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]: position = path.get_position(t) if position is None: continue - # print(f"\treserving pos/t for {agent_id}: {position} @ {t}") position_at_time = PositionAtTime(position, t) if position_at_time not in positions_at_time: positions_at_time[position_at_time] = AgentId(agent_id) @@ -75,8 +73,9 @@ 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}") + if verbose: + 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)) @@ -126,16 +125,16 @@ def get_next_node_to_expand(self) -> Optional[ConstraintTreeNode]: return None return heapq.heappop(self.nodes_to_expand) + """ + Add a node to the tree and generate children if needed. Returns true if the node is a solution, false otherwise. + """ 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. - """ heapq.heappush(self.nodes_to_expand, node) + """ + Get the constraints that were applied to all parent nodes starting with the node at the provided parent_index. + """ def get_ancestor_constraints(self, parent_index: int) -> list[AppliedConstraint]: - """ - Get the constraints that were applied to all parent nodes starting with the node at the provided parent_index. - """ constraints: list[AppliedConstraint] = [] while parent_index != -1: node = self.expanded_nodes[parent_index] @@ -146,10 +145,10 @@ def get_ancestor_constraints(self, parent_index: int) -> list[AppliedConstraint] parent_index = node.parent_idx return constraints + """ + Add an expanded node to the tree. Returns the index of this node in the expanded nodes dictionary. + """ 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. - """ node_idx = len(self.expanded_nodes) self.expanded_nodes[node_idx] = node return node_idx diff --git a/tests/test_conflict_based_search.py b/tests/test_conflict_based_search.py new file mode 100644 index 0000000000..6cde25434f --- /dev/null +++ b/tests/test_conflict_based_search.py @@ -0,0 +1,92 @@ +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Grid, + NodePath, + ObstacleArrangement, + Position, +) +from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal +from PathPlanning.TimeBasedPathPlanning import ConflictBasedSearch as m +from PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar import SpaceTimeAStar +from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner +from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner +import numpy as np +import conftest +import pytest + + +@pytest.mark.parametrize("single_agent_planner", [SpaceTimeAStar, SafeIntervalPathPlanner]) +def test_no_constraints(single_agent_planner): + # Test that planner succeeds with no constraints + grid_side_length = 21 + start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(4)] + + 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.NONE, + ) + + start_and_goals: list[StartAndGoal] + paths: list[NodePath] + start_and_goals, paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) + + # All paths should start at the specified position and reach the goal + for i, start_and_goal in enumerate(start_and_goals): + assert paths[i].path[0].position == start_and_goal.start + assert paths[i].path[-1].position == start_and_goal.goal + +@pytest.mark.parametrize("single_agent_planner", [SpaceTimeAStar, SafeIntervalPathPlanner]) +def test_narrow_corridor(single_agent_planner): + # Test a case that requires a few constraints + grid_side_length = 21 + start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(4)] + + 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, + ) + + start_and_goals: list[StartAndGoal] + paths: list[NodePath] + start_and_goals, paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) + + # All paths should start at the specified position and reach the goal + for i, start_and_goal in enumerate(start_and_goals): + assert paths[i].path[0].position == start_and_goal.start + assert paths[i].path[-1].position == start_and_goal.goal + +@pytest.mark.parametrize("single_agent_planner", [SpaceTimeAStar, SafeIntervalPathPlanner]) +def test_hallway_pass(single_agent_planner: SingleAgentPlanner): + # Test that search finds a path that requires a robot to temporarily move away from its goal + grid_side_length = 21 + 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))] + 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.HALLWAY, + ) + + start_and_goals: list[StartAndGoal] + paths: list[NodePath] + start_and_goals, paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) + + # All paths should start at the specified position and reach the goal + for i, start_and_goal in enumerate(start_and_goals): + assert paths[i].path[0].position == start_and_goal.start + assert paths[i].path[-1].position == start_and_goal.goal + +if __name__ == "__main__": + m.show_animation = False + conftest.run_this_test(__file__) From 2c84447f771568faea3ee007d7e868e21700e72a Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 14:12:08 -0500 Subject: [PATCH 16/29] cleaner return type from .plan() --- .../TimeBasedPathPlanning/BaseClasses.py | 4 +- .../ConflictBasedSearch.py | 17 ++++--- .../TimeBasedPathPlanning/Plotting.py | 50 ++++++++++--------- .../PriorityBasedPlanner.py | 11 ++-- 4 files changed, 43 insertions(+), 39 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 0e304efeac..f32d4203e9 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -43,8 +43,8 @@ class MultiAgentPlanner(ABC): @staticmethod @abstractmethod - def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]: + def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> dict[AgentId, NodePath]: """ - Plan for all agents. Returned paths are in order corresponding to the returned list of `StartAndGoal` objects + Plan for all agents. Returned paths found for each agent """ pass \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 610701ac48..f550067ca2 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -34,11 +34,9 @@ class ConflictBasedSearch(MultiAgentPlanner): @staticmethod - def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]: + def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> dict[AgentId, NodePath]: """ Generate a path from the start to the goal for each agent in the `start_and_goals` list. - Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans - corresponds to the order of the `start_and_goals` list. """ print(f"Using single-agent planner: {single_agent_planner_class}") @@ -72,7 +70,10 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c print(f"\nFound a path with constraints after {constraint_tree.expanded_node_count()} expansions") print(f"Final cost: {constraint_tree_node.cost}") print(f"Number of constraints on solution: {len(constraint_tree_node.all_constraints)}") - return (start_and_goals, [constraint_tree_node.paths[start_and_goal.agent_id] for start_and_goal in start_and_goals]) + final_paths = {} + for start_and_goal in start_and_goals: + final_paths[start_and_goal.agent_id] = constraint_tree_node.paths[start_and_goal.agent_id] + return final_paths if not isinstance(constraint_tree_node.constraint, ForkingConstraint): raise ValueError(f"Expected a ForkingConstraint, but got: {constraint_tree_node.constraint}") @@ -89,7 +90,6 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c # Deepcopy to update with applied constraint and new paths applied_constraint_parent = deepcopy(constraint_tree_node) - # Copy paths for child node - we just need to update constrained agent's path applied_constraint_parent.paths[constrained_agent.agent] = new_path if verbose: @@ -163,6 +163,7 @@ class Scenario(Enum): scenario = Scenario.HALLWAY_CROSS verbose = False show_animation = True +use_sipp = False # Condition here mainly to appease the linter np.random.seed(42) # For reproducibility def main(): grid_side_length = 21 @@ -187,16 +188,16 @@ def main(): obstacle_arrangement=obstacle_arrangement, ) + single_agent_planner = SafeIntervalPathPlanner if use_sipp else SpaceTimeAStar 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) + paths = ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, verbose) runtime = time.time() - start_time print(f"\nPlanning took: {runtime:.5f} seconds") if verbose: print(f"Paths:") - for path in paths: + for path in paths.values(): print(f"{path}\n") if not show_animation: diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index a7de25e4b2..29e2183431 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -7,6 +7,7 @@ ) from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal from PathPlanning.TimeBasedPathPlanning.Node import NodePath +from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId ''' Plot a single agent path. @@ -50,7 +51,7 @@ def PlotNodePath(grid: Grid, start: Position, goal: Position, path: NodePath): ''' Plot a series of agent paths. ''' -def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[NodePath]): +def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: dict[AgentId, NodePath]): fig = plt.figure(figsize=(10, 7)) ax = fig.add_subplot( @@ -64,19 +65,21 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N ax.set_yticks(np.arange(0, grid.grid_size[1], 1)) # Plot start and goal positions for each agent - colors = [] # generated randomly in loop + colors = {} # generated randomly in loop. Maps agent id to color markers = ['D', 's', '^', 'o', 'p'] # Different markers for visual distinction # Create plots for start and goal positions start_and_goal_plots = [] - for i, path in enumerate(paths): - marker_idx = i % len(markers) - agent_id = start_and_goals[i].agent_id - start = start_and_goals[i].start - goal = start_and_goals[i].goal + for agent_id, path in paths.items(): + marker_idx = agent_id % len(markers) + start_and_goal = next((elem for elem in start_and_goals if elem.agent_id == agent_id), None) + if not start_and_goal: + raise RuntimeError(f"Failed to get start and goal for agent {agent_id}") + start = start_and_goal.start + goal = start_and_goal.goal color = np.random.rand(3,) - colors.append(color) + colors[agent_id] = color sg_plot, = ax.plot([], [], markers[marker_idx], c=color, ms=15, label=f"Agent {agent_id} Start/Goal") sg_plot.set_data([start.x, goal.x], [start.y, goal.y]) @@ -86,12 +89,11 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles") # Create plots for each agent's path - path_plots = [] - for i, path in enumerate(paths): - agent_id = start_and_goals[i].agent_id - path_plot, = ax.plot([], [], "o", c=colors[i], ms=10, + path_plots = {} + for agent_id, path in paths.items(): + path_plot, = ax.plot([], [], "o", c=colors[agent_id], ms=10, label=f"Agent {agent_id} Path") - path_plots.append(path_plot) + path_plots[agent_id] = path_plot ax.legend(bbox_to_anchor=(1.05, 1)) @@ -103,32 +105,32 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N ) # Find the maximum time across all paths - max_time = max(path.goal_reached_time() for path in paths) + max_time = max(path.goal_reached_time() for path in paths.values()) # Animation loop - for i in range(0, max_time + 1): + for t in range(0, max_time + 1): # Update obstacle positions - obs_positions = grid.get_obstacle_positions_at_time(i) + obs_positions = grid.get_obstacle_positions_at_time(t) obs_points.set_data(obs_positions[0], obs_positions[1]) # Update each agent's position - for (j, path) in enumerate(paths): + for agent_id, path in paths.items(): path_positions = [] - if i <= path.goal_reached_time(): - res = path.get_position(i) + if t <= path.goal_reached_time(): + res = path.get_position(t) if not res: - print(path) - print(i) - path_position = path.get_position(i) + print(f"Error getting position for agent {agent_id} at time {t}") + print(t) + path_position = path.get_position(t) if not path_position: - raise Exception(f"Path position not found for time {i}.") + raise Exception(f"Path position not found for time {t}.") # Verify position is valid assert not path_position in obs_positions assert not path_position in path_positions path_positions.append(path_position) - path_plots[j].set_data([path_position.x], [path_position.y]) + path_plots[agent_id].set_data([path_position.x], [path_position.y]) plt.pause(0.2) diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index f297992867..6253e223c5 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -18,12 +18,13 @@ from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths +from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId import time class PriorityBasedPlanner(MultiAgentPlanner): @staticmethod - def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]: + def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> dict[AgentId, NodePath]: """ Generate a path from the start to the goal for each agent in the `start_and_goals` list. Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans @@ -40,7 +41,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c key=lambda item: item.distance_start_to_goal(), reverse=True) - paths = [] + paths = {} for start_and_goal in start_and_goals: if verbose: print(f"\nPlanning for agent: {start_and_goal}" ) @@ -54,9 +55,9 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c agent_index = start_and_goal.agent_id grid.reserve_path(path, agent_index) - paths.append(path) + paths[start_and_goal.agent_id] =path - return (start_and_goals, paths) + return paths verbose = False show_animation = True @@ -76,7 +77,7 @@ def main(): ) start_time = time.time() - start_and_goals, paths = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) + paths = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) runtime = time.time() - start_time print(f"\nPlanning took: {runtime:.5f} seconds") From e839b3aa3923e9d5b2ce7f33c561ab7969a1c810 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 14:35:56 -0500 Subject: [PATCH 17/29] fix tests + break up monster func --- .../ConflictBasedSearch.py | 14 ++- .../TimeBasedPathPlanning/ConstraintTree.py | 118 ++++++++++-------- tests/test_conflict_based_search.py | 27 ++-- tests/test_priority_based_planner.py | 18 +-- 4 files changed, 95 insertions(+), 82 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index f550067ca2..be9671d9ae 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -1,5 +1,5 @@ """ -Conflict Based Search generates paths in 2 dimensions (x, y, time) for a set of agents. It does +Conflict Based Search generates paths in 3 dimensions (x, y, time) for a set of agents. It does so by performing searches on two levels. The top level search applies constraints that agents must avoid, and the bottom level performs a single-agent search for individual agents given the constraints provided by the top level search. Initially, paths are generated for each agent @@ -105,14 +105,18 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c constraint_tree.add_node_to_tree(new_constraint_tree_node) raise RuntimeError("No solution found") - + + @staticmethod def get_agents_start_and_goal(start_and_goal_list: list[StartAndGoal], target_index: AgentId) -> StartAndGoal: + """ + Returns the start and goal of a specific agent + """ for item in start_and_goal_list: if item.agent_id == target_index: return item raise RuntimeError(f"Could not find agent with index {target_index} in {start_and_goal_list}") - + @staticmethod def plan_for_agent(constrained_agent: ConstraintTreeNode, all_constraints: list[AppliedConstraint], constraint_tree: ConstraintTree, @@ -121,7 +125,9 @@ def plan_for_agent(constrained_agent: ConstraintTreeNode, single_agent_planner_class: SingleAgentPlanner, start_and_goals: list[StartAndGoal], verbose: False) -> Optional[tuple[list[StartAndGoal], list[NodePath]]]: - + """ + Attempt to generate a path plan for a single agent + """ num_expansions = constraint_tree.expanded_node_count() if num_expansions % 50 == 0: print(f"Expanded {num_expansions} nodes so far...") diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index d7eb7a12f2..47f6f1fea5 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -46,68 +46,76 @@ def __lt__(self, other) -> bool: return self.cost < other.cost def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]: + """ + Check paths for any constraints, and if any are found return the earliest one. + """ 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): - possible_constraints: list[ForkingConstraint] = [] - 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 - position_at_time = PositionAtTime(position, t) - if position_at_time not in positions_at_time: - positions_at_time[position_at_time] = AgentId(agent_id) - - # edge conflict - 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: - if verbose: - 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)) - )) - possible_constraints.append(new_constraint) - continue - - # double reservation at a (cell, time) combination - if positions_at_time[position_at_time] != agent_id: - conflicting_agent_id = positions_at_time[position_at_time] - - constraint = Constraint(position=position, time=t) - possible_constraints.append(ForkingConstraint(( - ConstrainedAgent(agent_id, constraint), ConstrainedAgent(conflicting_agent_id, constraint) - ))) - continue - if 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: - if verbose: - print(f"\tFound edge conflict constraint: {constraint}") - return constraint - # if none, then return first normal constraint - if verbose: - print(f"\tReturning normal constraint: {possible_constraints[0]}") - return possible_constraints[0] + possible_constraints: list[ForkingConstraint] = self.check_for_constraints_at_time(positions_at_time, t, verbose) + if not possible_constraints: + continue + + 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: + if verbose: + print(f"\tFound edge conflict constraint: {constraint}") + return constraint + # if none, then return first normal constraint + if verbose: + print(f"\tReturning normal constraint: {possible_constraints[0]}") + return possible_constraints[0] return None + def check_for_constraints_at_time(self, positions_at_time: dict[PositionAtTime, AgentId], t: int, verbose: bool) -> list[ForkingConstraint]: + """ + Check for constraints between paths at a particular time step + """ + possible_constraints: list[ForkingConstraint] = [] + for agent_id, path in self.paths.items(): + + position = path.get_position(t) + if position is None: + continue + position_at_time = PositionAtTime(position, t) + if position_at_time not in positions_at_time: + positions_at_time[position_at_time] = AgentId(agent_id) + + # double reservation at a (cell, time) combination + if positions_at_time[position_at_time] != agent_id: + conflicting_agent_id = positions_at_time[position_at_time] + constraint = Constraint(position=position, time=t) + possible_constraints.append(ForkingConstraint(( + ConstrainedAgent(agent_id, constraint), ConstrainedAgent(conflicting_agent_id, constraint) + ))) + + # Check for edge conflicts (can only happen after first time step) + if t == 0: + continue + last_position = path.get_position(t - 1) + 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: + if verbose: + 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)) + )) + possible_constraints.append(new_constraint) + + return possible_constraints + class ConstraintTree: # Child nodes have been created (Maps node_index to ConstraintTreeNode) diff --git a/tests/test_conflict_based_search.py b/tests/test_conflict_based_search.py index 6cde25434f..6592c71f75 100644 --- a/tests/test_conflict_based_search.py +++ b/tests/test_conflict_based_search.py @@ -29,14 +29,13 @@ def test_no_constraints(single_agent_planner): obstacle_arrangement=ObstacleArrangement.NONE, ) - start_and_goals: list[StartAndGoal] paths: list[NodePath] - start_and_goals, paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) + paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) # All paths should start at the specified position and reach the goal - for i, start_and_goal in enumerate(start_and_goals): - assert paths[i].path[0].position == start_and_goal.start - assert paths[i].path[-1].position == start_and_goal.goal + for start_and_goal in start_and_goals: + assert paths[start_and_goal.agent_id].path[0].position == start_and_goal.start + assert paths[start_and_goal.agent_id].path[-1].position == start_and_goal.goal @pytest.mark.parametrize("single_agent_planner", [SpaceTimeAStar, SafeIntervalPathPlanner]) def test_narrow_corridor(single_agent_planner): @@ -53,14 +52,13 @@ def test_narrow_corridor(single_agent_planner): obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR, ) - start_and_goals: list[StartAndGoal] paths: list[NodePath] - start_and_goals, paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) + paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) # All paths should start at the specified position and reach the goal - for i, start_and_goal in enumerate(start_and_goals): - assert paths[i].path[0].position == start_and_goal.start - assert paths[i].path[-1].position == start_and_goal.goal + for start_and_goal in start_and_goals: + assert paths[start_and_goal.agent_id].path[0].position == start_and_goal.start + assert paths[start_and_goal.agent_id].path[-1].position == start_and_goal.goal @pytest.mark.parametrize("single_agent_planner", [SpaceTimeAStar, SafeIntervalPathPlanner]) def test_hallway_pass(single_agent_planner: SingleAgentPlanner): @@ -78,14 +76,13 @@ def test_hallway_pass(single_agent_planner: SingleAgentPlanner): obstacle_arrangement=ObstacleArrangement.HALLWAY, ) - start_and_goals: list[StartAndGoal] paths: list[NodePath] - start_and_goals, paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) + paths = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) # All paths should start at the specified position and reach the goal - for i, start_and_goal in enumerate(start_and_goals): - assert paths[i].path[0].position == start_and_goal.start - assert paths[i].path[-1].position == start_and_goal.goal + for start_and_goal in start_and_goals: + assert paths[start_and_goal.agent_id].path[0].position == start_and_goal.start + assert paths[start_and_goal.agent_id].path[-1].position == start_and_goal.goal if __name__ == "__main__": m.show_animation = False diff --git a/tests/test_priority_based_planner.py b/tests/test_priority_based_planner.py index e2ff602d88..2e648d1f63 100644 --- a/tests/test_priority_based_planner.py +++ b/tests/test_priority_based_planner.py @@ -6,15 +6,18 @@ ) from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal from PathPlanning.TimeBasedPathPlanning import PriorityBasedPlanner as m +from PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar import SpaceTimeAStar from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner +from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner import numpy as np import conftest +import pytest - -def test_1(): +@pytest.mark.parametrize("single_agent_planner", [SpaceTimeAStar, SafeIntervalPathPlanner]) +def test_1(single_agent_planner: SingleAgentPlanner): 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, 11)] obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)] grid = Grid( @@ -26,14 +29,13 @@ def test_1(): m.show_animation = False - start_and_goals: list[StartAndGoal] paths: list[NodePath] - start_and_goals, paths = m.PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, False) + paths = m.PriorityBasedPlanner.plan(grid, start_and_goals, single_agent_planner, False) # All paths should start at the specified position and reach the goal - for i, start_and_goal in enumerate(start_and_goals): - assert paths[i].path[0].position == start_and_goal.start - assert paths[i].path[-1].position == start_and_goal.goal + for start_and_goal in start_and_goals: + assert paths[start_and_goal.agent_id].path[0].position == start_and_goal.start + assert paths[start_and_goal.agent_id].path[-1].position == start_and_goal.goal if __name__ == "__main__": conftest.run_this_test(__file__) From bf030c22a1b5825717fa6416fd602b13d75a7837 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 14:50:17 -0500 Subject: [PATCH 18/29] small clean --- .../ConflictBasedSearch.py | 2 +- .../GridWithDynamicObstacles.py | 25 ++++--------------- 2 files changed, 6 insertions(+), 21 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index be9671d9ae..084fadc2b5 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -169,7 +169,7 @@ class Scenario(Enum): scenario = Scenario.HALLWAY_CROSS verbose = False show_animation = True -use_sipp = False # Condition here mainly to appease the linter +use_sipp = True # Condition here mainly to appease the linter np.random.seed(42) # For reproducibility def main(): grid_side_length = 21 diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 1f8323f7dc..64768a3e96 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -35,10 +35,7 @@ class ObstacleArrangement(Enum): 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)] - for x in range(arr.shape[0]): - for y in range(arr.shape[1]): - arr[x, y] = [] + 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: @@ -55,9 +52,11 @@ 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 + # Constraint points generated by ConflictBasedSearch that are currently applied applied_constraints: list[AppliedConstraint] = [] + # Note - this is separate from reservation_matrix because multiple robots can have the same + # constraints at the same (x, y, t) tuple + constraint_points: np.ndarray # Number of time steps in the simulation time_limit: int @@ -377,22 +376,13 @@ def get_safe_intervals(self, agent_idx: int) -> np.ndarray: def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Interval]: vals = self.reservation_matrix[cell.x, cell.y, :] - had_constraints = False - for constraint_set in self.constraint_points[cell.x, cell.y]: for constraint in constraint_set: if constraint.constrained_agent != agent_idx: continue if constraint.constraint.position != cell: 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 + 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) @@ -420,10 +410,6 @@ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Int # both the time step when it is entering the cell, and the time step when it is leaving the cell. intervals = [interval for interval in intervals if interval.start_time != interval.end_time] - # if had_constraints: - # print(f"agent {agent_idx}") - # print("\t\tconstraints: ", self.constraint_points[cell.x, cell.y]) - # print("\t\tintervals: ", intervals) return intervals """ @@ -434,7 +420,6 @@ 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: From ad9f39f4cbecfb00cb5cf2ce42aefeeda198ce00 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 14:57:36 -0500 Subject: [PATCH 19/29] more todos --- .../GridWithDynamicObstacles.py | 32 ++++++------------- .../TimeBasedPathPlanning/SafeInterval.py | 5 +-- 2 files changed, 11 insertions(+), 26 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 64768a3e96..e1efbe7623 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -44,6 +44,9 @@ def empty_3d_array_of_sets(x: int, y: int, z: int) -> np.ndarray: arr[:] = [[[set() for _ in range(z)] for _ in range(y)] for _ in range(x)] return arr +# To ensure we don't collide with an agent's index +OBSTACLE_INDEX: int = 123456 + class Grid: # Set in constructor grid_size: np.ndarray @@ -95,27 +98,12 @@ def __init__( elif obstacle_arrangement == ObstacleArrangement.NONE: self.obstacle_paths = [] - 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 - - 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 path in self.obstacle_paths: 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 + self.reservation_matrix[path[t - 1].x, path[t - 1].y, t] = OBSTACLE_INDEX + self.reservation_matrix[position.x, position.y, t] = OBSTACLE_INDEX """ Generate dynamic obstacles that move around the grid. Initial positions and movements are random @@ -375,6 +363,8 @@ def get_safe_intervals(self, agent_idx: int) -> np.ndarray: """ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Interval]: vals = self.reservation_matrix[cell.x, cell.y, :] + # Find where the array is zero + zero_mask = (vals == 0) for constraint_set in self.constraint_points[cell.x, cell.y]: for constraint in constraint_set: @@ -382,10 +372,8 @@ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Int continue if constraint.constraint.position != cell: continue - vals[constraint.constraint.time] = 99999 # TODO: no magic numbers - - # Find where the array is zero - zero_mask = (vals == 0) + # Mark this cell as constrained + zero_mask[constraint.constraint.time] = 0 # Identify transitions between zero and nonzero elements diff = np.diff(zero_mask.astype(int)) diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index f16ed223fc..8a46b2a45d 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -63,11 +63,8 @@ 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) - open_set: list[SIPPNode] = [] first_node_interval = safe_intervals[start.x, start.y][0] heapq.heappush( @@ -110,7 +107,7 @@ def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: b for child in SafeIntervalPathPlanner.generate_successors(grid, goal, expanded_node, expanded_idx, safe_intervals, visited_intervals, agent_idx): heapq.heappush(open_set, child) - raise Exception("No path found") + raise RuntimeError("No path found") """ Generate list of possible successors of the provided `parent_node` that are worth expanding From 93a0edd5f0addad41e5395cd78e96b368655885e Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 15:45:06 -0500 Subject: [PATCH 20/29] lint stuff --- .../TimeBasedPathPlanning/BaseClasses.py | 8 +++---- .../ConflictBasedSearch.py | 9 ++++--- .../TimeBasedPathPlanning/ConstraintTree.py | 23 +++++++++--------- .../GridWithDynamicObstacles.py | 24 +++++++++---------- PathPlanning/TimeBasedPathPlanning/Node.py | 6 ++--- .../TimeBasedPathPlanning/Plotting.py | 16 ++++++------- .../PriorityBasedPlanner.py | 4 ++-- .../TimeBasedPathPlanning/SafeInterval.py | 4 ++-- .../TimeBasedPathPlanning/SpaceTimeAStar.py | 2 +- tests/test_safe_interval_path_planner.py | 2 +- tests/test_space_time_astar.py | 2 +- 11 files changed, 50 insertions(+), 50 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index f32d4203e9..184e344525 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -18,7 +18,7 @@ class SingleAgentPlanner(ABC): """ Base class for single agent planners """ - + @staticmethod @abstractmethod def plan(grid: Grid, start: Position, goal: Position, agent_idx: AgentId, verbose: bool = False) -> NodePath: @@ -39,12 +39,12 @@ def distance_start_to_goal(self) -> float: class MultiAgentPlanner(ABC): """ Base class for multi-agent planners - """ - + """ + @staticmethod @abstractmethod def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> dict[AgentId, NodePath]: """ Plan for all agents. Returned paths found for each agent """ - pass \ No newline at end of file + pass diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 084fadc2b5..00f30c3028 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -20,7 +20,6 @@ ObstacleArrangement, Position, ) -from typing import Optional from PathPlanning.TimeBasedPathPlanning.BaseClasses import MultiAgentPlanner, StartAndGoal from PathPlanning.TimeBasedPathPlanning.Node import NodePath from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner @@ -53,7 +52,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c print(f"\nAgent {agent_idx} path:\n {path}") constraint_tree = ConstraintTree(initial_solution) - attempted_constraint_combos = set() + attempted_constraint_combos: set = set() while constraint_tree.nodes_to_expand: constraint_tree_node = constraint_tree.get_next_node_to_expand() @@ -124,7 +123,7 @@ def plan_for_agent(constrained_agent: ConstraintTreeNode, grid: Grid, single_agent_planner_class: SingleAgentPlanner, start_and_goals: list[StartAndGoal], - verbose: False) -> Optional[tuple[list[StartAndGoal], list[NodePath]]]: + verbose: False) -> tuple[list[StartAndGoal], list[NodePath]] | None: """ Attempt to generate a path plan for a single agent """ @@ -202,7 +201,7 @@ def main(): print(f"\nPlanning took: {runtime:.5f} seconds") if verbose: - print(f"Paths:") + print("Paths:") for path in paths.values(): print(f"{path}\n") @@ -212,4 +211,4 @@ def main(): PlotNodePaths(grid, start_and_goals, paths) if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 47f6f1fea5..6226bfc3d5 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from typing import Optional, TypeAlias +from typing import TypeAlias import heapq from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position, PositionAtTime @@ -27,8 +27,8 @@ class AppliedConstraint: @dataclass class ConstraintTreeNode: - parent_idx = int - constraint: Optional[ForkingConstraint | AppliedConstraint] + parent_idx: int + constraint: ForkingConstraint | AppliedConstraint | None paths: dict[AgentId, NodePath] cost: int @@ -45,7 +45,7 @@ def __lt__(self, other) -> bool: return len(self.all_constraints) < len(other.all_constraints) return self.cost < other.cost - def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]: + def get_constraint_point(self, verbose = False) -> ForkingConstraint | None: """ Check paths for any constraints, and if any are found return the earliest one. """ @@ -98,6 +98,8 @@ def check_for_constraints_at_time(self, positions_at_time: dict[PositionAtTime, if t == 0: continue last_position = path.get_position(t - 1) + if not position: + raise RuntimeError(f"Failed to get position for agent {agent_id} at time {t-1}") 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: @@ -109,7 +111,7 @@ def check_for_constraints_at_time(self, positions_at_time: dict[PositionAtTime, 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(agent_id, Constraint(position_at_time.position, position_at_time.time)), ConstrainedAgent(conflicting_agent_id1, Constraint(position=last_position, time=t)) )) possible_constraints.append(new_constraint) @@ -121,24 +123,23 @@ 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] + nodes_to_expand = list[ConstraintTreeNode] def __init__(self, initial_solution: dict[AgentId, NodePath]): - 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]: + def get_next_node_to_expand(self) -> ConstraintTreeNode | None: if not self.nodes_to_expand: return None return heapq.heappop(self.nodes_to_expand) - + """ Add a node to the tree and generate children if needed. Returns true if the node is a solution, false otherwise. """ def add_node_to_tree(self, node: ConstraintTreeNode) -> bool: heapq.heappush(self.nodes_to_expand, node) - + """ Get the constraints that were applied to all parent nodes starting with the node at the provided parent_index. """ @@ -162,4 +163,4 @@ def add_expanded_node(self, node: ConstraintTreeNode) -> int: return node_idx def expanded_node_count(self) -> int: - return len(self.expanded_nodes) \ No newline at end of file + return len(self.expanded_nodes) diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index e1efbe7623..0d2e459a34 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -193,7 +193,7 @@ def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]: obstacle_paths.append(path) return obstacle_paths - + def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Position]]: obstacle_paths = [] @@ -228,39 +228,39 @@ def generate_hallway_obstacles(self) -> list[list[Position]]: 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)): + for _ 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): @@ -308,7 +308,7 @@ def valid_position(self, position: Position, t: int, agent_idx: int) -> bool: """ def valid_obstacle_position(self, position: Position, t: int) -> bool: return ( - self.valid_position(position, t) + self.valid_position(position, t, OBSTACLE_INDEX) and position not in self.obstacle_avoid_points ) @@ -399,7 +399,7 @@ 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] return intervals - + """ Reserve an agent's path in the grid. Raises an exception if the agent's index is 0, or if a position is already reserved by a different agent. @@ -407,7 +407,7 @@ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Int def reserve_path(self, node_path: NodePath, agent_index: int): if agent_index == 0: raise Exception("Agent index cannot be 0") - + for i, node in enumerate(node_path.path): reservation_finish_time = node.time + 1 if i < len(node_path.path) - 1: diff --git a/PathPlanning/TimeBasedPathPlanning/Node.py b/PathPlanning/TimeBasedPathPlanning/Node.py index f311319d39..26bc4cedc5 100644 --- a/PathPlanning/TimeBasedPathPlanning/Node.py +++ b/PathPlanning/TimeBasedPathPlanning/Node.py @@ -1,7 +1,7 @@ from dataclasses import dataclass from functools import total_ordering import numpy as np -from typing import Sequence +from collections.abc import Sequence @dataclass(order=True) class Position: @@ -35,7 +35,7 @@ class PositionAtTime: def __hash__(self): return hash((self.position, self.time)) - + def __eq__(self, other): return self.position == other.position and self.time == other.time @@ -107,4 +107,4 @@ def __repr__(self): repr_string = "" for i, node in enumerate(self.path): repr_string += f"{i}: {node}\n" - return repr_string \ No newline at end of file + return repr_string diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index 29e2183431..2c4f6b54d1 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -77,10 +77,10 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: dict[A raise RuntimeError(f"Failed to get start and goal for agent {agent_id}") start = start_and_goal.start goal = start_and_goal.goal - + color = np.random.rand(3,) colors[agent_id] = color - sg_plot, = ax.plot([], [], markers[marker_idx], c=color, ms=15, + sg_plot, = ax.plot([], [], markers[marker_idx], c=color, ms=15, label=f"Agent {agent_id} Start/Goal") sg_plot.set_data([start.x, goal.x], [start.y, goal.y]) start_and_goal_plots.append(sg_plot) @@ -91,7 +91,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: dict[A # Create plots for each agent's path path_plots = {} for agent_id, path in paths.items(): - path_plot, = ax.plot([], [], "o", c=colors[agent_id], ms=10, + path_plot, = ax.plot([], [], "o", c=colors[agent_id], ms=10, label=f"Agent {agent_id} Path") path_plots[agent_id] = path_plot @@ -112,7 +112,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: dict[A # Update obstacle positions obs_positions = grid.get_obstacle_positions_at_time(t) obs_points.set_data(obs_positions[0], obs_positions[1]) - + # Update each agent's position for agent_id, path in paths.items(): path_positions = [] @@ -126,12 +126,12 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: dict[A raise Exception(f"Path position not found for time {t}.") # Verify position is valid - assert not path_position in obs_positions - assert not path_position in path_positions + assert path_position not in obs_positions + assert path_position not in path_positions path_positions.append(path_position) path_plots[agent_id].set_data([path_position.x], [path_position.y]) - + plt.pause(0.2) - plt.show() \ No newline at end of file + plt.show() diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index 6253e223c5..47cdf45086 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -83,7 +83,7 @@ def main(): print(f"\nPlanning took: {runtime:.5f} seconds") if verbose: - print(f"Paths:") + print("Paths:") for path in paths: print(f"{path}\n") @@ -93,4 +93,4 @@ def main(): PlotNodePaths(grid, start_and_goals, paths) if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index 8a46b2a45d..b37594bb44 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -24,7 +24,7 @@ from dataclasses import dataclass from functools import total_ordering import time -from typing import Sequence +from collections.abc import Sequence @dataclass() # Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because @@ -38,7 +38,7 @@ class SIPPNode(Node): class SIPPNodePath(NodePath): def __init__(self, path: Sequence[SIPPNode], expanded_node_count: int): super().__init__(path, expanded_node_count) - + self.positions_at_time = {} last_position = path[0].position for t in range(0, path[-1].time + 1): diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index 5136c557c1..aa4231a4bf 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -73,7 +73,7 @@ def plan(grid: Grid, start: Position, goal: Position, agent_index: int, verbose: @staticmethod def generate_successors( grid: Grid, goal: Position, parent_node: Node, parent_node_idx: int, agent_index: int, verbose: bool, expanded_set: set[Node] - ) -> Generator[Node, None, None]: + ) -> Generator[Node]: diffs = [ Position(0, 0), Position(1, 0), diff --git a/tests/test_safe_interval_path_planner.py b/tests/test_safe_interval_path_planner.py index bbcd4e427c..1901927531 100644 --- a/tests/test_safe_interval_path_planner.py +++ b/tests/test_safe_interval_path_planner.py @@ -18,7 +18,7 @@ def test_1(): ) m.show_animation = False - path = m.SafeIntervalPathPlanner.plan(grid, start, goal) + path = m.SafeIntervalPathPlanner.plan(grid, start, goal, 0) # path should have 31 entries assert len(path.path) == 31 diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py index 1194c61d2e..eefddca270 100644 --- a/tests/test_space_time_astar.py +++ b/tests/test_space_time_astar.py @@ -18,7 +18,7 @@ def test_1(): ) m.show_animation = False - path = m.SpaceTimeAStar.plan(grid, start, goal) + path = m.SpaceTimeAStar.plan(grid, start, goal, 0) # path should have 28 entries assert len(path.path) == 31 From 075d1318dc6aba6f35e7ce8579d134730117141a Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 15:55:56 -0500 Subject: [PATCH 21/29] mypy --- .../ConflictBasedSearch.py | 25 ++++++++++++++----- .../TimeBasedPathPlanning/ConstraintTree.py | 7 +++--- 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 00f30c3028..c4d1d1148c 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -20,14 +20,15 @@ ObstacleArrangement, Position, ) +import time + 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 +from PathPlanning.TimeBasedPathPlanning.ConstraintTree import AgentId, AppliedConstraint, ConstrainedAgent, ConstraintTree, ConstraintTreeNode, ForkingConstraint class ConflictBasedSearch(MultiAgentPlanner): @@ -56,13 +57,16 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c while constraint_tree.nodes_to_expand: constraint_tree_node = constraint_tree.get_next_node_to_expand() + if constraint_tree_node is None: + raise RuntimeError("No more nodes to expand in the constraint tree.") + ancestor_constraints = constraint_tree.get_ancestor_constraints(constraint_tree_node.parent_idx) if verbose: print(f"Expanding node with constraint {constraint_tree_node.constraint} and parent {constraint_tree_node.parent_idx}") print(f"\tCOST: {constraint_tree_node.cost}") - if constraint_tree_node is None: + if constraint_tree_node.constraint 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! @@ -83,7 +87,16 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c all_constraints = ancestor_constraints all_constraints.append(applied_constraint) - new_path = ConflictBasedSearch.plan_for_agent(constrained_agent, all_constraints, constraint_tree, attempted_constraint_combos, grid, single_agent_planner_class, start_and_goals, verbose) + new_path = ConflictBasedSearch.plan_for_agent( + constrained_agent, + all_constraints, + constraint_tree, + attempted_constraint_combos, + grid, + single_agent_planner_class, + start_and_goals, + verbose + ) if not new_path: continue @@ -116,14 +129,14 @@ def get_agents_start_and_goal(start_and_goal_list: list[StartAndGoal], target_in raise RuntimeError(f"Could not find agent with index {target_index} in {start_and_goal_list}") @staticmethod - def plan_for_agent(constrained_agent: ConstraintTreeNode, + def plan_for_agent(constrained_agent: ConstrainedAgent, all_constraints: list[AppliedConstraint], constraint_tree: ConstraintTree, attempted_constraint_combos: set, grid: Grid, single_agent_planner_class: SingleAgentPlanner, start_and_goals: list[StartAndGoal], - verbose: False) -> tuple[list[StartAndGoal], list[NodePath]] | None: + verbose: bool) -> NodePath | None: """ Attempt to generate a path plan for a single agent """ diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 6226bfc3d5..0922346da4 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -98,7 +98,7 @@ def check_for_constraints_at_time(self, positions_at_time: dict[PositionAtTime, if t == 0: continue last_position = path.get_position(t - 1) - if not position: + if not last_position: raise RuntimeError(f"Failed to get position for agent {agent_id} at time {t-1}") new_position_at_last_time = PositionAtTime(position, t-1) old_position_at_new_time = PositionAtTime(last_position, t) @@ -123,10 +123,11 @@ 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 = list[ConstraintTreeNode] + nodes_to_expand: list[ConstraintTreeNode] def __init__(self, initial_solution: dict[AgentId, NodePath]): self.expanded_nodes = {} + self.nodes_to_expand = [] heapq.heappush(self.nodes_to_expand, ConstraintTreeNode(initial_solution, -1, [])) def get_next_node_to_expand(self) -> ConstraintTreeNode | None: @@ -137,7 +138,7 @@ def get_next_node_to_expand(self) -> ConstraintTreeNode | None: """ Add a node to the tree and generate children if needed. Returns true if the node is a solution, false otherwise. """ - def add_node_to_tree(self, node: ConstraintTreeNode) -> bool: + def add_node_to_tree(self, node: ConstraintTreeNode): heapq.heappush(self.nodes_to_expand, node) """ From 99d5af921229f2b672e47c959678645c7b875496 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 16:15:49 -0500 Subject: [PATCH 22/29] codestyle --- PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py | 2 +- PathPlanning/TimeBasedPathPlanning/ConstraintTree.py | 5 ++--- .../TimeBasedPathPlanning/GridWithDynamicObstacles.py | 4 ++-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index c4d1d1148c..115155cb1b 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -45,7 +45,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c # Generate initial solution (no constraints) for start_and_goal in start_and_goals: path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose) - initial_solution[AgentId(start_and_goal.agent_id)] = path + initial_solution[start_and_goal.agent_id] = path if verbose: print("Initial solution:") diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 0922346da4..4d6713038c 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -1,10 +1,9 @@ from dataclasses import dataclass -from typing import TypeAlias import heapq from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position, PositionAtTime -AgentId: TypeAlias = int +type AgentId = int @dataclass(frozen=True) class Constraint: @@ -84,7 +83,7 @@ def check_for_constraints_at_time(self, positions_at_time: dict[PositionAtTime, continue position_at_time = PositionAtTime(position, t) if position_at_time not in positions_at_time: - positions_at_time[position_at_time] = AgentId(agent_id) + positions_at_time[position_at_time] = agent_id # double reservation at a (cell, time) combination if positions_at_time[position_at_time] != agent_id: diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 0d2e459a34..8589c3f0db 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -219,13 +219,13 @@ 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) From a362ea4d48209d7cff4e2a4e7fe7160f82f32dee Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 16:26:14 -0500 Subject: [PATCH 23/29] thanks claude --- PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 115155cb1b..6a4cfeed47 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -66,8 +66,6 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c print(f"Expanding node with constraint {constraint_tree_node.constraint} and parent {constraint_tree_node.parent_idx}") print(f"\tCOST: {constraint_tree_node.cost}") - if constraint_tree_node.constraint 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! print(f"\nFound a path with constraints after {constraint_tree.expanded_node_count()} expansions") From 7d9161afb2b2862d0260777b2930b9dd31e58341 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 16:26:26 -0500 Subject: [PATCH 24/29] docs --- .../time_based_grid_search_main.rst | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst index a44dd20a97..41b51612cd 100644 --- a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst +++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst @@ -100,9 +100,21 @@ Dynamic Obstacles: .. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner/priority_planner.gif +Conflict Based Search +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This multi-agent planning algorithm performs searches at two levels. The lower level generates paths for individual agents using a set of constraints provided by the top level search. The top level search generates plans for each agent, and checks for constraints. If any are found, two new nodes are created with one of the two conflicting agents constrained to avoid the (x, y, t) tuple where the conflict occurred. + +This process is repeated until the top level search finds a set of path plans with no conflicts. The top level search chooses nodes to expand based on the cost of the paths it has for each agent. This guarantees the solution found is optimal. + + +.. image:: TODO +waiting on this PR: https://github.com/AtsushiSakai/PythonRoboticsGifs/pull/14 + References ~~~~~~~~~~~ - `Cooperative Pathfinding `__ - `SIPP: Safe Interval Path Planning for Dynamic Environments `__ -- `Prioritized Planning Algorithms for Trajectory Coordination of Multiple Mobile Robots `__ \ No newline at end of file +- `Prioritized Planning Algorithms for Trajectory Coordination of Multiple Mobile Robots `__ +- `Conflict-Based Search For Optimal Multi-Agent Path Finding `__ From 4331b7b87f1ebb5173550f9f16fa12deed850082 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 16:31:13 -0500 Subject: [PATCH 25/29] review bot --- PathPlanning/TimeBasedPathPlanning/ConstraintTree.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 4d6713038c..12c492817e 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -44,6 +44,16 @@ def __lt__(self, other) -> bool: return len(self.all_constraints) < len(other.all_constraints) return self.cost < other.cost + def __le__(self, other) -> bool: + if self.cost == other.cost: + return len(self.all_constraints) <= len(other.all_constraints) + return self.cost < other.cost + + def __ge__(self, other) -> bool: + if self.cost == other.cost: + return len(self.all_constraints) >= len(other.all_constraints) + return self.cost > other.cost + def get_constraint_point(self, verbose = False) -> ForkingConstraint | None: """ Check paths for any constraints, and if any are found return the earliest one. From b43a4fcf58c9bc8a9356870ab68212003c16c9e7 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 16:34:45 -0500 Subject: [PATCH 26/29] docs --- .../time_based_grid_search/time_based_grid_search_main.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst index 41b51612cd..d3e395b532 100644 --- a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst +++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst @@ -107,9 +107,9 @@ This multi-agent planning algorithm performs searches at two levels. The lower l This process is repeated until the top level search finds a set of path plans with no conflicts. The top level search chooses nodes to expand based on the cost of the paths it has for each agent. This guarantees the solution found is optimal. +This algorithm can also solve some cases that a priority based planner cannot, such as the below example where one robot must move out of the way of another robot's path. Without cooperation across agent paths, this case cannot be solved. -.. image:: TODO -waiting on this PR: https://github.com/AtsushiSakai/PythonRoboticsGifs/pull/14 +TODO: image waiting on this PR: https://github.com/AtsushiSakai/PythonRoboticsGifs/pull/14 References ~~~~~~~~~~~ From 8447aed66708d15296f911528c6b1383a7c9ae68 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 18:19:27 -0500 Subject: [PATCH 27/29] add comparison test --- .../ConflictBasedSearch.py | 3 +- tests/test_conflict_based_search.py | 44 +++++++++++++++++++ 2 files changed, 45 insertions(+), 2 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index 6a4cfeed47..caef6a5596 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -68,9 +68,8 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c if not constraint_tree_node.constraint: # This means we found a solution! - print(f"\nFound a path with constraints after {constraint_tree.expanded_node_count()} expansions") + print(f"\nFound a path with {len(constraint_tree_node.all_constraints)} constraints after {constraint_tree.expanded_node_count()} expansions") print(f"Final cost: {constraint_tree_node.cost}") - print(f"Number of constraints on solution: {len(constraint_tree_node.all_constraints)}") final_paths = {} for start_and_goal in start_and_goals: final_paths[start_and_goal.agent_id] = constraint_tree_node.paths[start_and_goal.agent_id] diff --git a/tests/test_conflict_based_search.py b/tests/test_conflict_based_search.py index 6592c71f75..80fba70d81 100644 --- a/tests/test_conflict_based_search.py +++ b/tests/test_conflict_based_search.py @@ -6,6 +6,7 @@ ) from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal from PathPlanning.TimeBasedPathPlanning import ConflictBasedSearch as m +from PathPlanning.TimeBasedPathPlanning import PriorityBasedPlanner as m2 from PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar import SpaceTimeAStar from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner @@ -84,6 +85,49 @@ def test_hallway_pass(single_agent_planner: SingleAgentPlanner): assert paths[start_and_goal.agent_id].path[0].position == start_and_goal.start assert paths[start_and_goal.agent_id].path[-1].position == start_and_goal.goal +@pytest.mark.parametrize("single_agent_planner", [SpaceTimeAStar, SafeIntervalPathPlanner]) +def test_better_than_priority_based_planner(single_agent_planner: SingleAgentPlanner): + """ + Test that CBS returns a better solution than the priority based planner + """ + grid_side_length = 21 + + start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 3)] + 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.ARRANGEMENT1, + ) + + paths: dict[int, NodePath] + paths = m2.PriorityBasedPlanner.plan(grid, start_and_goals, single_agent_planner, False) + + # Re-initialize grid to clear reservations that PBP populates + grid = Grid( + np.array([grid_side_length, grid_side_length]), + num_obstacles=250, + obstacle_avoid_points=obstacle_avoid_points, + obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, + ) + + paths2: dict[int, NodePath] + paths2 = m.ConflictBasedSearch.plan(grid, start_and_goals, single_agent_planner, False) + + # All paths should start at the specified position and reach the goal + for start_and_goal in start_and_goals: + assert paths[start_and_goal.agent_id].path[0].position == start_and_goal.start + assert paths[start_and_goal.agent_id].path[-1].position == start_and_goal.goal + assert paths2[start_and_goal.agent_id].path[0].position == start_and_goal.start + assert paths2[start_and_goal.agent_id].path[-1].position == start_and_goal.goal + + pbp_cost = sum([path.goal_reached_time() for path in paths.values()]) + cbs_cost = sum([path.goal_reached_time() for path in paths2.values()]) + assert pbp_cost > cbs_cost, f"CBS cost {cbs_cost} should be less than PBP cost {pbp_cost}" + if __name__ == "__main__": m.show_animation = False + m2.show_animation = False conftest.run_this_test(__file__) From ec48aaa97d4135464ad4fb72bd086139a811f3ad Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 18:24:37 -0500 Subject: [PATCH 28/29] doc comments --- PathPlanning/TimeBasedPathPlanning/ConstraintTree.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py index 12c492817e..3e7b608813 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py +++ b/PathPlanning/TimeBasedPathPlanning/ConstraintTree.py @@ -3,22 +3,27 @@ from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position, PositionAtTime +# ID of an agent type AgentId = int +# An (x, y, t) tuple that exists in two or more agents' paths @dataclass(frozen=True) class Constraint: position: Position time: int +# An agent that is constrained (must avoid the constraint tuple) @dataclass class ConstrainedAgent: agent: AgentId constraint: Constraint +# A constraint to apply to the two colliding agents @dataclass class ForkingConstraint: constrained_agents: tuple[ConstrainedAgent, ConstrainedAgent] +# A constraint that has been applied to an agent @dataclass(frozen=True) class AppliedConstraint: constraint: Constraint @@ -26,10 +31,15 @@ class AppliedConstraint: @dataclass class ConstraintTreeNode: + # Index of the parent node parent_idx: int + # Either a constraint to apply (forking) which will result in two new nodes, a constraint that was already applied + # to generate this node (AppliedConstraint), or None if there are no constraints in this node's paths constraint: ForkingConstraint | AppliedConstraint | None + # Paths for the agents paths: dict[AgentId, NodePath] + # Cost of the solution at this node (sum of duration of each agent's path) cost: int def __init__(self, paths: dict[AgentId, NodePath], parent_idx: int, all_constraints: list[AppliedConstraint]): From 5b140f04b3f2287d828ea0de75455d7928a676e1 Mon Sep 17 00:00:00 2001 From: Jonathan Schwartz Date: Mon, 29 Dec 2025 18:37:47 -0500 Subject: [PATCH 29/29] more cleanup --- .../ConflictBasedSearch.py | 46 ++++++++++--------- .../GridWithDynamicObstacles.py | 30 +++++++----- 2 files changed, 43 insertions(+), 33 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py index caef6a5596..9855fe466f 100644 --- a/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py +++ b/PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py @@ -40,19 +40,7 @@ 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] = {} - - # Generate initial solution (no constraints) - for start_and_goal in start_and_goals: - path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose) - initial_solution[start_and_goal.agent_id] = 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) + constraint_tree: ConstraintTree = ConflictBasedSearch.initialize_constraint_tree(grid, start_and_goals, single_agent_planner_class, verbose) attempted_constraint_combos: set = set() while constraint_tree.nodes_to_expand: @@ -100,11 +88,6 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c # Deepcopy to update with applied constraint and new paths applied_constraint_parent = deepcopy(constraint_tree_node) applied_constraint_parent.paths[constrained_agent.agent] = new_path - - if verbose: - for (agent_idx, path) in applied_constraint_parent.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) @@ -152,10 +135,10 @@ def plan_for_agent(constrained_agent: ConstrainedAgent, 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) + grid.clear_constraint_reservations() + grid.apply_constraint_reservations(all_constraints) # Just plan for agent with new constraint start_and_goal: StartAndGoal = ConflictBasedSearch.get_agents_start_and_goal(start_and_goals, constrained_agent.agent) @@ -165,10 +148,31 @@ def plan_for_agent(constrained_agent: ConstrainedAgent, new_path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose) return new_path except Exception as e: + # Note: single agent planners can fail if constraints make planning for an agent impossible. The upper level search does not + # consider if a constraint will make it impossible for an agent to plan when adding a new constraint. if verbose: print(f"Error: {e}") return None + @staticmethod + def initialize_constraint_tree(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> ConstraintTree: + """ + Generate an initial solution by planning for each agent with no obstacles + """ + initial_solution: dict[AgentId, NodePath] = {} + + # Generate initial solution (no constraints) + for start_and_goal in start_and_goals: + path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose) + initial_solution[start_and_goal.agent_id] = path + + if verbose: + print("Initial solution:") + for (agent_idx, path) in initial_solution.items(): + print(f"\nAgent {agent_idx} path:\n {path}") + + return ConstraintTree(initial_solution) + class Scenario(Enum): # Five robots all trying to get through a single cell choke point to reach their goals NARROW_CORRIDOR = 0 diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 8589c3f0db..bee2999ace 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -59,7 +59,7 @@ class Grid: applied_constraints: list[AppliedConstraint] = [] # Note - this is separate from reservation_matrix because multiple robots can have the same # constraints at the same (x, y, t) tuple - constraint_points: np.ndarray + constraint_reservations: np.ndarray # Number of time steps in the simulation time_limit: int @@ -80,7 +80,7 @@ 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) + self.constraint_reservations = 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!") @@ -253,7 +253,7 @@ def generate_hallway_obstacles(self) -> list[list[Position]]: return obstacle_paths - def generate_temporary_obstacle(self, hallway_length: int = 3) -> list[list[Position]]: + def generate_temporary_obstacle(self) -> list[list[Position]]: """ Generates a temporary obstacle at (10, 10) that disappears at t=30 """ @@ -263,21 +263,27 @@ 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): + def apply_constraint_reservations(self, constraints: list[AppliedConstraint], verbose = False): + """ + Add new constraints and reserve them in the constraint_reservations map + """ self.applied_constraints.extend(constraints) 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 constraint not in self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]: + self.constraint_reservations[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]}") + print(f"\tExisting constraints: {self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]}") - def clear_constraint_points(self): + def clear_constraint_reservations(self): + """ + Clear all applied constraints and their associated reservations + """ 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) + if constraint in self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]: + self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time].remove(constraint) self.applied_constraints.clear() """ @@ -295,7 +301,7 @@ def valid_position(self, position: Position, t: int, agent_idx: int) -> bool: if not self.inside_grid_bounds(position): return False - constraints = self.constraint_points[position.x, position.y, t] + constraints = self.constraint_reservations[position.x, position.y, t] for constraint in constraints: if constraint.constrained_agent == agent_idx: return False @@ -366,7 +372,7 @@ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Int # Find where the array is zero zero_mask = (vals == 0) - for constraint_set in self.constraint_points[cell.x, cell.y]: + for constraint_set in self.constraint_reservations[cell.x, cell.y]: for constraint in constraint_set: if constraint.constrained_agent != agent_idx: continue