From 30b4de722ad40d7d9ba20a3d958dd15bbfca2634 Mon Sep 17 00:00:00 2001 From: anishk85 Date: Wed, 22 Oct 2025 15:02:17 +0530 Subject: [PATCH] added new algorithm fringe search --- PathPlanning/FringeSearch/__init__.py | 0 PathPlanning/FringeSearch/fringe_search.py | 401 ++++++++++++++++++ README.md | 17 +- .../fringeSearch/fringeSearch_main.rst | 135 ++++++ .../5_path_planning/path_planning_main.rst | 1 + tests/test_fringe_search.py | 11 + 6 files changed, 564 insertions(+), 1 deletion(-) create mode 100644 PathPlanning/FringeSearch/__init__.py create mode 100644 PathPlanning/FringeSearch/fringe_search.py create mode 100644 docs/modules/5_path_planning/fringeSearch/fringeSearch_main.rst create mode 100644 tests/test_fringe_search.py diff --git a/PathPlanning/FringeSearch/__init__.py b/PathPlanning/FringeSearch/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/PathPlanning/FringeSearch/fringe_search.py b/PathPlanning/FringeSearch/fringe_search.py new file mode 100644 index 0000000000..64dfd2d8e1 --- /dev/null +++ b/PathPlanning/FringeSearch/fringe_search.py @@ -0,0 +1,401 @@ +""" +Fringe Search Path Planning + +author: Anish (@anishk85) + +See paper: Björnsson, Y.; Enzenberger, M.; Holte, R.; Schaeffer, J. (2005). +"Fringe Search: Beating A* at Pathfinding on Game Maps" + +Reference: + - https://webdocs.cs.ualberta.ca/~holte/Publications/fringe.pdf + +Fringe Search is a memory-efficient graph search algorithm that combines +the benefits of iterative deepening with A*. It maintains a small "fringe" +of nodes and iteratively searches with increasing f-cost thresholds. + +Key advantages: +- Lower memory usage than A* (no priority queue) +- Comparable or better performance on grid maps +- Simple implementation +""" + +import math +import matplotlib.pyplot as plt + +show_animation = True + +class Node: + """Node in the search grid.""" + + def __init__(self, x, y, cost, parent_index): + self.x = x + self.y = y + self.cost = cost # g-cost (cost from start) + self.parent_index = parent_index + + def __str__(self): + return f"({self.x},{self.y}) cost:{self.cost} parent:{self.parent_index}" + + +class FringeSearch: + """ + Fringe Search path planner. + + Uses iterative deepening with f-cost thresholds to find optimal paths + while using minimal memory compared to A*. + """ + + def __init__(self, ox, oy, resolution, robot_radius): + """ + Initialize Fringe Search planner. + + Args: + ox: List of obstacle x coordinates [m] + oy: List of obstacle y coordinates [m] + resolution: Grid resolution [m] + robot_radius: Robot radius [m] + """ + self.resolution = resolution + self.robot_radius = robot_radius + self.min_x = min(ox) + self.min_y = min(oy) + self.max_x = max(ox) + self.max_y = max(oy) + self.x_width = int((self.max_x - self.min_x) / self.resolution) + self.y_width = int((self.max_y - self.min_y) / self.resolution) + self.obstacle_map = self.calc_obstacle_map(ox, oy) + self.motion = self.get_motion_model() + + def planning(self, sx, sy, gx, gy): + """ + Execute Fringe Search path planning. + + Args: + sx: Start x position [m] + sy: Start y position [m] + gx: Goal x position [m] + gy: Goal y position [m] + + Returns: + rx: x position list of the final path + ry: y position list of the final path + """ + start_node = Node( + self.calc_xy_index(sx, self.min_x), + self.calc_xy_index(sy, self.min_y), + 0.0, + -1, + ) + goal_node = Node( + self.calc_xy_index(gx, self.min_x), + self.calc_xy_index(gy, self.min_y), + 0.0, + -1, + ) + + # Initialize data structures + fringe = [start_node] # The fringe list + cache = {} # Node cache: index -> (node, f_cost, in_fringe) + + start_index = self.calc_index(start_node) + h_start = self.calc_heuristic(start_node, goal_node) + cache[start_index] = (start_node, start_node.cost + h_start, True) + + flimit = h_start # Initial f-cost threshold + found = False + + if show_animation: + plt.figure(figsize=(10, 8)) + + iteration = 0 + while not found and fringe: + fmin = float('inf') # Minimum f-cost exceeding current threshold + + # Iterate through fringe with current threshold + fringe_copy = fringe.copy() + fringe.clear() + + for node in fringe_copy: + node_index = self.calc_index(node) + + if node_index not in cache: + continue + + _, f_cost, in_fringe = cache[node_index] + + if not in_fringe: + continue + + # If f-cost exceeds threshold, defer to next iteration + if f_cost > flimit: + fmin = min(f_cost, fmin) + fringe.append(node) + continue + + # Goal test + if node.x == goal_node.x and node.y == goal_node.y: + goal_node.parent_index = node.parent_index + goal_node.cost = node.cost + found = True + break + + # Mark as visited (remove from fringe) + cache[node_index] = (node, f_cost, False) + + # Visualization + if show_animation and iteration % 10 == 0: + self.plot_current_state(node, goal_node, cache) + + # Expand neighbors + for motion in self.motion: + n_node = Node( + node.x + motion[0], + node.y + motion[1], + node.cost + motion[2], + node_index, + ) + n_index = self.calc_index(n_node) + + # Check validity + if not self.verify_node(n_node): + continue + + # Calculate costs + g_cost = n_node.cost + h_cost = self.calc_heuristic(n_node, goal_node) + f_cost = g_cost + h_cost + + if n_index in cache: + cached_node, cached_f, cached_in_fringe = cache[n_index] + + # Update if we found a better path + if g_cost < cached_node.cost: + cache[n_index] = (n_node, f_cost, True) + if not cached_in_fringe: + fringe.append(n_node) + else: + # New node + cache[n_index] = (n_node, f_cost, True) + fringe.append(n_node) + + iteration += 1 + + if not found: + # Update threshold for next iteration + flimit = fmin + + # Reconstruct path + rx, ry = self.calc_final_path(goal_node, cache) + + if show_animation: + self.plot_final_path(rx, ry, sx, sy, gx, gy) + + return rx, ry + + def calc_final_path(self, goal_node, cache): + """ + Reconstruct path from goal to start. + + Args: + goal_node: Goal node + cache: Node cache dictionary + + Returns: + rx: x position list + ry: y position list + """ + rx = [self.calc_position(goal_node.x, self.min_x)] + ry = [self.calc_position(goal_node.y, self.min_y)] + parent_index = goal_node.parent_index + + while parent_index != -1: + if parent_index not in cache: + break + n, _, _ = cache[parent_index] + rx.append(self.calc_position(n.x, self.min_x)) + ry.append(self.calc_position(n.y, self.min_y)) + parent_index = n.parent_index + + return rx, ry + + def calc_heuristic(self, node, goal): + """ + Calculate heuristic (Euclidean distance). + + Args: + node: Current node + goal: Goal node + + Returns: + Heuristic value + """ + return math.hypot(node.x - goal.x, node.y - goal.y) + + def calc_index(self, node): + """Calculate grid index.""" + return node.y * self.x_width + node.x + + def calc_xy_index(self, position, min_pos): + """Convert position to grid index.""" + return int((position - min_pos) / self.resolution) + + def calc_position(self, index, min_pos): + """Convert grid index to position.""" + return index * self.resolution + min_pos + + def verify_node(self, node): + """Check if node is valid (within bounds and not obstacle).""" + if node.x < 0 or node.x >= self.x_width: + return False + if node.y < 0 or node.y >= self.y_width: + return False + if self.obstacle_map[node.x][node.y]: + return False + return True + + def calc_obstacle_map(self, ox, oy): + """ + Create obstacle map with inflation. + + Args: + ox: Obstacle x coordinates + oy: Obstacle y coordinates + + Returns: + 2D boolean obstacle map + """ + obstacle_map = [ + [False for _ in range(self.y_width)] for _ in range(self.x_width) + ] + + for x, y in zip(ox, oy): + ix = self.calc_xy_index(x, self.min_x) + iy = self.calc_xy_index(y, self.min_y) + if 0 <= ix < self.x_width and 0 <= iy < self.y_width: + obstacle_map[ix][iy] = True + + return obstacle_map + + @staticmethod + def get_motion_model(): + """ + Define 8-directional motion model. + + Returns: + List of [dx, dy, cost] movements + """ + motion = [ + [1, 0, 1.0], + [0, 1, 1.0], + [-1, 0, 1.0], + [0, -1, 1.0], + [-1, -1, math.sqrt(2)], + [-1, 1, math.sqrt(2)], + [1, -1, math.sqrt(2)], + [1, 1, math.sqrt(2)], + ] + return motion + + def plot_current_state(self, current, goal, cache): + """Visualize current search state.""" + plt.clf() + + # Plot obstacles + for x in range(self.x_width): + for y in range(self.y_width): + if self.obstacle_map[x][y]: + px = self.calc_position(x, self.min_x) + py = self.calc_position(y, self.min_y) + plt.plot(px, py, "sk", markersize=2) + + # Plot visited nodes + for node_index, (node, _, in_fringe) in cache.items(): + if not in_fringe: + px = self.calc_position(node.x, self.min_x) + py = self.calc_position(node.y, self.min_y) + plt.plot(px, py, "xc", markersize=1) + + # Plot current node + px = self.calc_position(current.x, self.min_x) + py = self.calc_position(current.y, self.min_y) + plt.plot(px, py, "og") + + # Plot goal + px = self.calc_position(goal.x, self.min_x) + py = self.calc_position(goal.y, self.min_y) + plt.plot(px, py, "xr") + + plt.grid(True) + plt.axis("equal") + plt.pause(0.001) + + def plot_final_path(self, rx, ry, sx, sy, gx, gy): + """Visualize final path.""" + plt.plot(rx, ry, "-r", linewidth=2, label="Fringe Search Path") + plt.plot(sx, sy, "og", markersize=10, label="Start") + plt.plot(gx, gy, "xr", markersize=10, label="Goal") + plt.legend() + plt.grid(True) + plt.axis("equal") + plt.title("Fringe Search Result") + plt.xlabel("X [m]") + plt.ylabel("Y [m]") + plt.show() + + +def main(): + """Run Fringe Search demonstration.""" + print(__file__ + " start!!") + + # Start and goal positions + sx = 10.0 # [m] + sy = 10.0 # [m] + gx = 50.0 # [m] + gy = 50.0 # [m] + grid_size = 2.0 # [m] + robot_radius = 1.0 # [m] + + # Set obstacle positions + ox, oy = [], [] + + # Boundary + for i in range(0, 60): + ox.append(i) + oy.append(0.0) + for i in range(0, 60): + ox.append(60.0) + oy.append(i) + for i in range(0, 61): + ox.append(i) + oy.append(60.0) + for i in range(0, 61): + ox.append(0.0) + oy.append(i) + + # Internal obstacles + for i in range(0, 40): + ox.append(20.0) + oy.append(i) + for i in range(0, 40): + ox.append(40.0) + oy.append(60.0 - i) + + if show_animation: + plt.plot(ox, oy, ".k") + plt.plot(sx, sy, "og") + plt.plot(gx, gy, "xr") + plt.grid(True) + plt.axis("equal") + + fringe = FringeSearch(ox, oy, grid_size, robot_radius) + rx, ry = fringe.planning(sx, sy, gx, gy) + + if rx: + print("Path found!") + print(f"Path length: {len(rx)} nodes") + else: + print("No path found!") + + +if __name__ == "__main__": + main() diff --git a/README.md b/README.md index 65445fa4ce..2c5e79a09e 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,8 @@ Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index. * [Potential Field algorithm](#potential-field-algorithm) * [Grid based coverage path planning](#grid-based-coverage-path-planning) * [Particle Swarm Optimization (PSO)](#particle-swarm-optimization-pso) - * [State Lattice Planning](#state-lattice-planning) + * [Fringe Search](#fringe-search) + * [State Lattice Planning](#state-lattice-planning) * [Biased polar sampling](#biased-polar-sampling) * [Lane sampling](#lane-sampling) * [Probabilistic Road-Map (PRM) planning](#probabilistic-road-map-prm-planning) @@ -373,6 +374,20 @@ References - [Kennedy, J.; Eberhart, R. (1995). "Particle Swarm Optimization"](https://ieeexplore.ieee.org/document/488968) +### Fringe Search +This is a memory-efficient grid-based path planning implementation using Fringe Search. + +![FringeSearch](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FringeSearch/animation.gif) + +Fringe Search is an informed search algorithm that combines iterative deepening with A*-style heuristics. It maintains a small "fringe" of nodes at the search frontier and iteratively explores paths with increasing f-cost thresholds. + +Key advantages include significantly lower memory usage compared to A* (no priority queue), while maintaining comparable performance and guaranteeing optimal paths on grid maps. + +References + +- [Fringe Search: Beating A* at Pathfinding on Game Maps](https://webdocs.cs.ualberta.ca/~holte/Publications/fringe.pdf) + +- [Björnsson, Y.; Enzenberger, M.; Holte, R.; Schaeffer, J. (2005)](https://cdn.aaai.org/AAAI/2005/AAAI05-184.pdf) ## State Lattice Planning diff --git a/docs/modules/5_path_planning/fringeSearch/fringeSearch_main.rst b/docs/modules/5_path_planning/fringeSearch/fringeSearch_main.rst new file mode 100644 index 0000000000..ad7e1dbc31 --- /dev/null +++ b/docs/modules/5_path_planning/fringeSearch/fringeSearch_main.rst @@ -0,0 +1,135 @@ +Fringe Search Path Planning +---------------------------- + +This is a memory-efficient grid-based path planning implementation using Fringe Search. + +Fringe Search is an informed search algorithm that combines iterative deepening with A*-style heuristics. It maintains a "fringe" of nodes at the search frontier and iteratively explores paths with increasing f-cost thresholds, providing memory efficiency comparable to iterative deepening while maintaining near-optimal performance. + +Code Link +~~~~~~~~~ + +.. autofunction:: PathPlanning.FringeSearch.fringe_search.main + +Algorithm Overview +~~~~~~~~~~~~~~~~~~ + +Fringe Search operates in iterations with increasing f-cost thresholds: + +1. **Initialize**: Start with initial node in fringe, set f-limit to heuristic value +2. **Iterate**: Process nodes in fringe with f-cost ≤ current threshold +3. **Expand**: Generate successor nodes and calculate their f-costs +4. **Defer**: Nodes exceeding threshold are deferred to next iteration +5. **Update**: Increase threshold to minimum deferred f-cost +6. **Repeat**: Continue until goal is found + +Animation +~~~~~~~~~ + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FringeSearch/animation.gif + +Mathematical Foundation +~~~~~~~~~~~~~~~~~~~~~~~ + +F-cost Calculation +^^^^^^^^^^^^^^^^^^ + +The f-cost combines path cost and heuristic: + +.. math:: + + f(n) = g(n) + h(n) + +Where: + +- :math:`f(n)` = estimated total cost through node n +- :math:`g(n)` = actual cost from start to node n +- :math:`h(n)` = heuristic estimate from node n to goal + +Threshold Update +^^^^^^^^^^^^^^^^ + +After each iteration, the threshold is updated: + +.. math:: + + f_{limit}^{new} = \min_{n \in Fringe} f(n) \text{ where } f(n) > f_{limit}^{old} + +This ensures monotonic increase in threshold while exploring all nodes within each bound. + +Heuristic Function +^^^^^^^^^^^^^^^^^^ + +Euclidean distance is used as the heuristic: + +.. math:: + + h(n) = \sqrt{(x_n - x_g)^2 + (y_n - y_g)^2} + +Where :math:`(x_n, y_n)` is the current position and :math:`(x_g, y_g)` is the goal position. + +Key Features +~~~~~~~~~~~~ + +**Memory Efficiency** + - No priority queue (unlike A*) + - Small fringe list maintained + - O(b*d) memory complexity where b=branching factor, d=depth + +**Cache Management** + - Tracks visited nodes with (node, f-cost, in_fringe) tuples + - Efficient lookup for duplicate detection + - Updates costs when better paths found + +**Iterative Deepening** + - Gradually increases f-cost threshold + - Avoids exploring high-cost regions prematurely + - Guarantees optimality (with admissible heuristic) + +Advantages +~~~~~~~~~~ + +- **Low memory usage**: No large priority queue or open list +- **Optimal paths**: Guarantees optimal solution with admissible heuristic +- **Simple implementation**: Easier to understand than A* +- **Cache-friendly**: Better memory locality than A* +- **Performance**: Competitive with A* on grid maps + +Disadvantages +~~~~~~~~~~~~~ + +- **Iteration overhead**: May re-expand nodes across iterations +- **Grid-based**: Best suited for uniform-cost grids +- **Not anytime**: Must complete iteration to get valid path +- **Threshold selection**: Performance sensitive to f-limit updates + +Comparison with A* +~~~~~~~~~~~~~~~~~~ + ++------------------+------------------+------------------+ +| Aspect | A* | Fringe Search | ++==================+==================+==================+ +| Memory | O(b^d) | O(b*d) | ++------------------+------------------+------------------+ +| Data Structure | Priority Queue | Simple List | ++------------------+------------------+------------------+ +| Optimality | Yes | Yes | ++------------------+------------------+------------------+ +| Speed | Fast | Comparable | ++------------------+------------------+------------------+ +| Complexity | Medium | Low | ++------------------+------------------+------------------+ + +Use Cases +~~~~~~~~~ + +- **Memory-constrained systems**: Embedded robotics, mobile devices +- **Grid-based games**: Pathfinding in tile-based games +- **Large maps**: When A* priority queue becomes too large +- **Educational**: Simpler to teach than A* + +Reference +~~~~~~~~~ + +- `Fringe Search Paper - Björnsson et al. (2005) `__ +- `A* search algorithm - Wikipedia `__ +- Björnsson, Y.; Enzenberger, M.; Holte, R.; Schaeffer, J. (2005). "Fringe Search: Beating A* at Pathfinding on Game Maps". Proceedings of the National Conference on Artificial Intelligence. diff --git a/docs/modules/5_path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst index 5132760dc5..eb43ddbdbe 100644 --- a/docs/modules/5_path_planning/path_planning_main.rst +++ b/docs/modules/5_path_planning/path_planning_main.rst @@ -20,6 +20,7 @@ Path planning is the ability of a robot to search feasible and efficient path to vrm_planner/vrm_planner rrt/rrt particleSwarmOptimization/particleSwarmOptimization + fringeSearch/fringeSearch cubic_spline/cubic_spline bspline_path/bspline_path catmull_rom_spline/catmull_rom_spline diff --git a/tests/test_fringe_search.py b/tests/test_fringe_search.py new file mode 100644 index 0000000000..ffa87576b5 --- /dev/null +++ b/tests/test_fringe_search.py @@ -0,0 +1,11 @@ +import conftest # Add root path +from PathPlanning.FringeSearch import fringe_search + + +def test_1(): + fringe_search.show_animation = False + fringe_search.main() + + +if __name__ == "__main__": + conftest.run_this_test(__file__)