From 94b0c1af66fb061a1c79e0710ea44d829c45bc97 Mon Sep 17 00:00:00 2001 From: anishk85 Date: Sun, 5 Oct 2025 13:21:53 +0530 Subject: [PATCH 1/3] added pso algorithm also known as bird flocking algorithm --- .gitignore | 1 + .../particle_swarm_optimization.py | 312 ++++++++++++++++++ README.md | 19 ++ .../particleSwarmOptimization.rst | 153 +++++++++ tests/test_particle_swarm_optimization.py | 11 + 5 files changed, 496 insertions(+) create mode 100755 PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py create mode 100644 docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization.rst create mode 100755 tests/test_particle_swarm_optimization.py diff --git a/.gitignore b/.gitignore index c971b8f9c5..8a0b1257b5 100644 --- a/.gitignore +++ b/.gitignore @@ -15,6 +15,7 @@ _build/ # Distribution / packaging .Python env/ +venv/ build/ develop-eggs/ dist/ diff --git a/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py b/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py new file mode 100755 index 0000000000..3d497dfe28 --- /dev/null +++ b/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py @@ -0,0 +1,312 @@ +""" +Particle Swarm Optimization (PSO) Path Planning + +author: Anish (@anishk85) + +See Wikipedia article (https://en.wikipedia.org/wiki/Particle_swarm_optimization) + +References: + - Kennedy, J.; Eberhart, R. (1995). "Particle Swarm Optimization" + - Shi, Y.; Eberhart, R. (1998). "A Modified Particle Swarm Optimizer" + - https://machinelearningmastery.com/a-gentle-introduction-to-particle-swarm-optimization/ + +This implementation uses PSO to find collision-free paths by treating +path planning as an optimization problem where particles explore the +search space to minimize distance to target while avoiding obstacles. +""" +import numpy as np +import matplotlib +import matplotlib.pyplot as plt +import matplotlib.animation as animation +import matplotlib.patches as patches +import signal +import sys + +# Add show_animation flag for consistency with other planners +show_animation = True + +def signal_handler(sig, frame): + print('\nExiting...') + plt.close('all') + sys.exit(0) + +signal.signal(signal.SIGINT, signal_handler) + +class Particle: + def __init__(self, search_bounds, spawn_bounds): + self.search_bounds = search_bounds + self.max_velocity = np.array([(b[1] - b[0]) * 0.05 for b in search_bounds]) + self.position = np.array([np.random.uniform(b[0], b[1]) for b in spawn_bounds]) + self.velocity = np.random.randn(2) * 0.1 + self.pbest_position = self.position.copy() + self.pbest_value = np.inf + self.path = [self.position.copy()] + + def update_velocity(self, gbest_pos, w, c1, c2): + """Update particle velocity using PSO equation: + v = w*v + c1*r1*(pbest - x) + c2*r2*(gbest - x) + """ + r1 = np.random.rand(2) + r2 = np.random.rand(2) + + cognitive = c1 * r1 * (self.pbest_position - self.position) + social = c2 * r2 * (gbest_pos - self.position) + + self.velocity = w * self.velocity + cognitive + social + self.velocity = np.clip(self.velocity, -self.max_velocity, self.max_velocity) + + def update_position(self): + self.position = self.position + self.velocity + + # Keep in bounds + for i in range(2): + self.position[i] = np.clip(self.position[i], + self.search_bounds[i][0], + self.search_bounds[i][1]) + + self.path.append(self.position.copy()) + + +class PSOSwarm: + def __init__(self, n_particles, max_iter, target, search_bounds, + spawn_bounds, obstacles): + self.n_particles = n_particles + self.max_iter = max_iter + self.target = np.array(target) + self.obstacles = obstacles + self.search_bounds = search_bounds + + # PSO parameters + self.w_start = 0.9 # Initial inertia weight + self.w_end = 0.4 # Final inertia weight + self.c1 = 1.5 # Cognitive coefficient + self.c2 = 1.5 # Social coefficient + + # Initialize particles + self.particles = [Particle(search_bounds, spawn_bounds) + for _ in range(n_particles)] + + self.gbest_position = None + self.gbest_value = np.inf + self.gbest_path = [] + self.iteration = 0 + + def fitness(self, pos): + """Calculate fitness - distance to target + obstacle penalty""" + dist = np.linalg.norm(pos - self.target) + + # Obstacle penalty + penalty = 0 + for ox, oy, r in self.obstacles: + obs_dist = np.linalg.norm(pos - np.array([ox, oy])) + if obs_dist < r: + penalty += 1000 # Inside obstacle + elif obs_dist < r + 5: + penalty += 50 / (obs_dist - r + 0.1) # Too close + + return dist + penalty + + def check_collision(self, start, end, obstacle): + """Check if path from start to end hits obstacle using line-circle intersection""" + ox, oy, r = obstacle + center = np.array([ox, oy]) + + # Vector math for line-circle intersection + d = end - start + f = start - center + + a = np.dot(d, d) + b = 2 * np.dot(f, d) + c = np.dot(f, f) - r * r + + discriminant = b * b - 4 * a * c + + if discriminant < 0: + return False + + # Check if intersection on segment + t1 = (-b - np.sqrt(discriminant)) / (2 * a) + t2 = (-b + np.sqrt(discriminant)) / (2 * a) + + return (0 <= t1 <= 1) or (0 <= t2 <= 1) + + def step(self): + """Run one PSO iteration""" + if self.iteration >= self.max_iter: + return False + + # Update inertia weight (linear decay) + w = self.w_start - (self.w_start - self.w_end) * (self.iteration / self.max_iter) + + # Evaluate all particles + for particle in self.particles: + value = self.fitness(particle.position) + + # Update personal best + if value < particle.pbest_value: + particle.pbest_value = value + particle.pbest_position = particle.position.copy() + + # Update global best + if value < self.gbest_value: + self.gbest_value = value + self.gbest_position = particle.position.copy() + + if self.gbest_position is not None: + self.gbest_path.append(self.gbest_position.copy()) + + # Update particles + for particle in self.particles: + particle.update_velocity(self.gbest_position, w, self.c1, self.c2) + + # Predict next position + next_pos = particle.position + particle.velocity + + # Check collision + collision = False + for obs in self.obstacles: + if self.check_collision(particle.position, next_pos, obs): + collision = True + break + + if collision: + # Reduce velocity if collision detected + particle.velocity *= 0.2 + + particle.update_position() + + self.iteration += 1 + if show_animation and self.iteration % 20 == 0: + print(f"Iteration {self.iteration}/{self.max_iter}, Best: {self.gbest_value:.2f}") + return True + + +def main(): + """Test PSO path planning algorithm""" + print(__file__ + " start!!") + + # Set matplotlib backend for headless environments + if not show_animation: + matplotlib.use('Agg') # Use non-GUI backend for testing + + # Setup + N_PARTICLES = 15 + MAX_ITER = 150 + SEARCH_BOUNDS = [(-50, 50), (-50, 50)] + TARGET = [40, 35] + SPAWN_AREA = [(-45, -35), (-45, -35)] + OBSTACLES = [ + (10, 15, 8), + (-20, 0, 12), + (20, -25, 10), + (-5, -30, 7) + ] + + swarm = PSOSwarm( + n_particles=N_PARTICLES, + max_iter=MAX_ITER, + target=TARGET, + search_bounds=SEARCH_BOUNDS, + spawn_bounds=SPAWN_AREA, + obstacles=OBSTACLES + ) + + if show_animation: # pragma: no cover + # Visualization + fig, ax = plt.subplots(figsize=(10, 10)) + ax.set_xlim(SEARCH_BOUNDS[0]) + ax.set_ylim(SEARCH_BOUNDS[1]) + ax.set_title("PSO Path Planning with Collision Avoidance", fontsize=14) + ax.grid(True, alpha=0.3) + + # Draw obstacles + for ox, oy, r in OBSTACLES: + circle = patches.Circle((ox, oy), r, color='gray', alpha=0.7) + ax.add_patch(circle) + + # Draw spawn area + spawn_rect = patches.Rectangle( + (SPAWN_AREA[0][0], SPAWN_AREA[1][0]), + SPAWN_AREA[0][1] - SPAWN_AREA[0][0], + SPAWN_AREA[1][1] - SPAWN_AREA[1][0], + linewidth=2, edgecolor='green', facecolor='green', + alpha=0.2, label='Start Zone' + ) + ax.add_patch(spawn_rect) + + # Draw target + ax.plot(TARGET[0], TARGET[1], 'r*', markersize=20, label='Target') + + # Initialize plot elements + particles_scatter = ax.scatter([], [], c='blue', s=50, alpha=0.6, label='Particles') + gbest_scatter = ax.plot([], [], 'yo', markersize=12, label='Best Position')[0] + particle_paths = [ax.plot([], [], 'b-', lw=0.5, alpha=0.2)[0] for _ in range(N_PARTICLES)] + gbest_path_line = ax.plot([], [], 'y--', lw=2, alpha=0.8, label='Best Path')[0] + + iteration_text = ax.text(0.02, 0.95, '', transform=ax.transAxes, + fontsize=12, verticalalignment='top', + bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.5)) + + ax.legend(loc='upper right') + + def animate(frame): + if not swarm.step(): + return + + # Update particle positions + positions = np.array([p.position for p in swarm.particles]) + particles_scatter.set_offsets(positions) + + # Update particle paths + for i, particle in enumerate(swarm.particles): + if len(particle.path) > 1: + path = np.array(particle.path) + particle_paths[i].set_data(path[:, 0], path[:, 1]) + + # Update global best + if swarm.gbest_position is not None: + gbest_scatter.set_data([swarm.gbest_position[0]], + [swarm.gbest_position[1]]) + + if len(swarm.gbest_path) > 1: + gbest = np.array(swarm.gbest_path) + gbest_path_line.set_data(gbest[:, 0], gbest[:, 1]) + + # Update text + iteration_text.set_text( + f'Iteration: {swarm.iteration}/{MAX_ITER}\n' + f'Best Fitness: {swarm.gbest_value:.2f}' + ) + + return (particles_scatter, gbest_scatter, gbest_path_line, + iteration_text, *particle_paths) + + ani = animation.FuncAnimation( + fig, animate, frames=MAX_ITER, + interval=100, blit=True, repeat=False + ) + + plt.tight_layout() + plt.show() + else: + # Run without animation for testing + print("Running PSO algorithm without animation...") + iteration_count = 0 + while swarm.step(): + iteration_count += 1 + if iteration_count >= MAX_ITER: + break + + print(f"PSO completed after {iteration_count} iterations") + print(f"Best fitness: {swarm.gbest_value:.2f}") + if swarm.gbest_position is not None: + print(f"Best position: [{swarm.gbest_position[0]:.2f}, {swarm.gbest_position[1]:.2f}]") + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print("\nProgram interrupted by user") + plt.close('all') + sys.exit(0) \ No newline at end of file diff --git a/README.md b/README.md index d1b801f219..a26eb48a02 100644 --- a/README.md +++ b/README.md @@ -36,6 +36,7 @@ Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index. * [D* Lite algorithm](#d-lite-algorithm) * [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) * [Biased polar sampling](#biased-polar-sampling) * [Lane sampling](#lane-sampling) @@ -356,6 +357,24 @@ This is a 2D grid based coverage path planning simulation. ![PotentialField](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/GridBasedSweepCPP/animation.gif) +### Particle Swarm Optimization (PSO) + +This is a 2D path planning with Particle Swarm Optimization algorithm. + +![PSO](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ParticleSwarmOptimization/animation.gif) + +PSO is a metaheuristic optimization algorithm inspired by bird flocking behavior. In path planning, particles explore the search space to find collision-free paths while avoiding obstacles. + +The animation shows particles (blue dots) converging towards the optimal path (yellow line) from start (green area) to goal (red star). + +Reference + +- [Particle swarm optimization - Wikipedia](https://en.wikipedia.org/wiki/Particle_swarm_optimization) + +- [Kennedy, J.; Eberhart, R. (1995). "Particle Swarm Optimization"](https://ieeexplore.ieee.org/document/488968) + + + ## State Lattice Planning This script is a path planning code with state lattice planning. diff --git a/docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization.rst b/docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization.rst new file mode 100644 index 0000000000..64c3286cb6 --- /dev/null +++ b/docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization.rst @@ -0,0 +1,153 @@ +.. _particle_swarm_optimization: + +Particle Swarm Optimization Path Planning +------------------------------------------ + +This is a 2D path planning implementation using Particle Swarm Optimization (PSO). + +PSO is a metaheuristic optimization algorithm inspired by the social behavior of bird flocking or fish schooling. In path planning, particles represent potential solutions that explore the search space to find collision-free paths from start to goal. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ParticleSwarmOptimization/animation.gif + +Algorithm Overview +++++++++++++++++++ + +The PSO algorithm maintains a swarm of particles that move through the search space according to simple mathematical rules: + +1. **Initialization**: Particles are randomly distributed near the start position +2. **Evaluation**: Each particle's fitness is calculated based on distance to goal and obstacle penalties +3. **Update**: Particles adjust their velocities based on: + - Personal best position (cognitive component) + - Global best position (social component) + - Current velocity (inertia component) +4. **Movement**: Particles move to new positions and check for collisions +5. **Convergence**: Process repeats until maximum iterations or goal is reached + +Mathematical Foundation ++++++++++++++++++++++++ + +The core PSO velocity update equation is: + +.. math:: + + v_{i}(t+1) = w \cdot v_{i}(t) + c_1 \cdot r_1 \cdot (p_{i} - x_{i}(t)) + c_2 \cdot r_2 \cdot (g - x_{i}(t)) + +Where: +- :math:`v_{i}(t)` = velocity of particle i at time t +- :math:`x_{i}(t)` = position of particle i at time t +- :math:`w` = inertia weight (controls exploration vs exploitation) +- :math:`c_1` = cognitive coefficient (attraction to personal best) +- :math:`c_2` = social coefficient (attraction to global best) +- :math:`r_1, r_2` = random numbers in [0,1] +- :math:`p_{i}` = personal best position of particle i +- :math:`g` = global best position + +Position update: + +.. math:: + + x_{i}(t+1) = x_{i}(t) + v_{i}(t+1) + +Fitness Function +++++++++++++++++ + +The fitness function combines distance to target with obstacle penalties: + +.. math:: + + f(x) = ||x - x_{goal}|| + \sum_{j} P_{obs}(x, O_j) + +Where: +- :math:`||x - x_{goal}||` = Euclidean distance to goal +- :math:`P_{obs}(x, O_j)` = penalty for obstacle j +- :math:`O_j` = obstacle j with position and radius + +The obstacle penalty function is defined as: + +.. math:: + + P_{obs}(x, O_j) = \begin{cases} + 1000 & \text{if } ||x - O_j|| < r_j \text{ (inside obstacle)} \\ + \frac{50}{||x - O_j|| - r_j + 0.1} & \text{if } r_j \leq ||x - O_j|| < r_j + R_{influence} \text{ (near obstacle)} \\ + 0 & \text{if } ||x - O_j|| \geq r_j + R_{influence} \text{ (safe distance)} + \end{cases} + +Where: +- :math:`r_j` = radius of obstacle j +- :math:`R_{influence}` = influence radius (typically 5 units) + +Collision Detection ++++++++++++++++++++ + +Line-circle intersection is used to detect collisions between particle paths and circular obstacles: + +.. math:: + + ||P_0 + t \cdot \vec{d} - C|| = r + +Where: +- :math:`P_0` = start point of path segment +- :math:`\vec{d}` = direction vector of path +- :math:`C` = obstacle center +- :math:`r` = obstacle radius +- :math:`t \in [0,1]` = parameter along line segment + +Algorithm Parameters +++++++++++++++++++++ + +Key parameters affecting performance: + +- **Number of particles** (n_particles): More particles = better exploration but slower +- **Maximum iterations** (max_iter): More iterations = better convergence but slower +- **Inertia weight** (w): High = exploration, Low = exploitation +- **Cognitive coefficient** (c1): Attraction to personal best +- **Social coefficient** (c2): Attraction to global best + +Typical values: +- n_particles: 20-50 +- max_iter: 100-300 +- w: 0.9 → 0.4 (linearly decreasing) +- c1, c2: 1.5-2.0 + +Advantages +++++++++++ + +- **Global optimization**: Can escape local minima unlike gradient-based methods +- **No derivatives needed**: Works with non-differentiable fitness landscapes +- **Parallel exploration**: Multiple particles search simultaneously +- **Simple implementation**: Few parameters and straightforward logic +- **Flexible**: Easily adaptable to different environments and constraints + +Disadvantages ++++++++++++++ + +- **Stochastic**: Results may vary between runs +- **Parameter sensitive**: Performance heavily depends on parameter tuning +- **No optimality guarantee**: Metaheuristic without convergence proof +- **Computational cost**: Requires many fitness evaluations +- **Prone to stagnation**: Premature convergence where the entire swarm can get trapped in a local minimum if exploration is insufficient + +Code Link ++++++++++ + +.. autofunction:: PathPlanning.ParticleSwarmOptimization.particle_swarm_optimization.main + +Usage Example ++++++++++++++ + +.. code-block:: python + + import matplotlib.pyplot as plt + from PathPlanning.ParticleSwarmOptimization.particle_swarm_optimization import main + + # Run PSO path planning with visualization + main() + +References +++++++++++ + +- `Particle swarm optimization - Wikipedia `__ +- Kennedy, J.; Eberhart, R. (1995). "Particle Swarm Optimization". Proceedings of IEEE International Conference on Neural Networks. IV. pp. 1942–1948. +- Shi, Y.; Eberhart, R. (1998). "A Modified Particle Swarm Optimizer". IEEE International Conference on Evolutionary Computation. +- `A Gentle Introduction to Particle Swarm Optimization `__ +- Clerc, M.; Kennedy, J. (2002). "The particle swarm - explosion, stability, and convergence in a multidimensional complex space". IEEE Transactions on Evolutionary Computation. 6 (1): 58–73. \ No newline at end of file diff --git a/tests/test_particle_swarm_optimization.py b/tests/test_particle_swarm_optimization.py new file mode 100755 index 0000000000..95720022f6 --- /dev/null +++ b/tests/test_particle_swarm_optimization.py @@ -0,0 +1,11 @@ +import conftest # Add root path to sys.path +from PathPlanning.ParticleSwarmOptimization import particle_swarm_optimization as m + + +def test_1(): + m.show_animation = False + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__) \ No newline at end of file From c20cf35578e3406c2a945f6454f811ee2ab5518d Mon Sep 17 00:00:00 2001 From: anishk85 Date: Sun, 5 Oct 2025 14:18:00 +0530 Subject: [PATCH 2/3] fix: Resolve linting issues in PSO implementation --- .../particle_swarm_optimization.py | 66 +++++++++++++++---- 1 file changed, 54 insertions(+), 12 deletions(-) diff --git a/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py b/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py index 3d497dfe28..030af7c354 100755 --- a/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py +++ b/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py @@ -10,8 +10,8 @@ - Shi, Y.; Eberhart, R. (1998). "A Modified Particle Swarm Optimizer" - https://machinelearningmastery.com/a-gentle-introduction-to-particle-swarm-optimization/ -This implementation uses PSO to find collision-free paths by treating -path planning as an optimization problem where particles explore the +This implementation uses PSO to find collision-free paths by treating +path planning as an optimization problem where particles explore the search space to minimize distance to target while avoiding obstacles. """ import numpy as np @@ -107,7 +107,16 @@ def fitness(self, pos): return dist + penalty def check_collision(self, start, end, obstacle): - """Check if path from start to end hits obstacle using line-circle intersection""" + """Check if path from start to end hits obstacle using line-circle intersection + + Args: + start: Starting position (numpy array) + end: Ending position (numpy array) + obstacle: Tuple (ox, oy, r) representing obstacle center and radius + + Returns: + bool: True if collision detected, False otherwise + """ ox, oy, r = obstacle center = np.array([ox, oy]) @@ -116,6 +125,12 @@ def check_collision(self, start, end, obstacle): f = start - center a = np.dot(d, d) + + # FIX: Guard against zero-length steps to prevent ZeroDivisionError + if a < 1e-10: # Near-zero length step + # Check if start point is inside obstacle + return np.linalg.norm(f) <= r + b = 2 * np.dot(f, d) c = np.dot(f, f) - r * r @@ -125,13 +140,18 @@ def check_collision(self, start, end, obstacle): return False # Check if intersection on segment - t1 = (-b - np.sqrt(discriminant)) / (2 * a) - t2 = (-b + np.sqrt(discriminant)) / (2 * a) + sqrt_discriminant = np.sqrt(discriminant) + t1 = (-b - sqrt_discriminant) / (2 * a) + t2 = (-b + sqrt_discriminant) / (2 * a) return (0 <= t1 <= 1) or (0 <= t2 <= 1) def step(self): - """Run one PSO iteration""" + """Run one PSO iteration + + Returns: + bool: True if algorithm should continue, False if completed + """ if self.iteration >= self.max_iter: return False @@ -178,18 +198,31 @@ def step(self): self.iteration += 1 if show_animation and self.iteration % 20 == 0: print(f"Iteration {self.iteration}/{self.max_iter}, Best: {self.gbest_value:.2f}") + return True def main(): - """Test PSO path planning algorithm""" + """Run PSO path planning algorithm demonstration. + + This function demonstrates PSO-based path planning with the following setup: + - 15 particles exploring a (-50,50) x (-50,50) search space + - Start zone: (-45,-35) to (-35,-35) + - Target: (40, 35) + - 4 circular obstacles with collision avoidance + - Real-time visualization showing particle convergence (if show_animation=True) + - Headless mode support for testing (if show_animation=False) + + The algorithm runs for up to 150 iterations, displaying particle movement, + personal/global best positions, and the evolving optimal path. + """ print(__file__ + " start!!") # Set matplotlib backend for headless environments if not show_animation: matplotlib.use('Agg') # Use non-GUI backend for testing - # Setup + # Setup parameters N_PARTICLES = 15 MAX_ITER = 150 SEARCH_BOUNDS = [(-50, 50), (-50, 50)] @@ -211,8 +244,9 @@ def main(): obstacles=OBSTACLES ) - if show_animation: # pragma: no cover - # Visualization + # pragma: no cover + if show_animation: + # Visualization setup fig, ax = plt.subplots(figsize=(10, 10)) ax.set_xlim(SEARCH_BOUNDS[0]) ax.set_ylim(SEARCH_BOUNDS[1]) @@ -250,8 +284,10 @@ def main(): ax.legend(loc='upper right') def animate(frame): + """Animation function for matplotlib FuncAnimation""" if not swarm.step(): - return + return (particles_scatter, gbest_scatter, gbest_path_line, + iteration_text, *particle_paths) # Update particle positions positions = np.array([p.position for p in swarm.particles]) @@ -281,13 +317,17 @@ def animate(frame): return (particles_scatter, gbest_scatter, gbest_path_line, iteration_text, *particle_paths) - ani = animation.FuncAnimation( + # Create animation and store reference to prevent garbage collection + animation_ref = animation.FuncAnimation( fig, animate, frames=MAX_ITER, interval=100, blit=True, repeat=False ) plt.tight_layout() plt.show() + + # Keep reference to prevent garbage collection + return animation_ref else: # Run without animation for testing print("Running PSO algorithm without animation...") @@ -301,6 +341,8 @@ def animate(frame): print(f"Best fitness: {swarm.gbest_value:.2f}") if swarm.gbest_position is not None: print(f"Best position: [{swarm.gbest_position[0]:.2f}, {swarm.gbest_position[1]:.2f}]") + + return None if __name__ == "__main__": From ca9e5fc71e609a0423fd94285167c1a2ec02d6de Mon Sep 17 00:00:00 2001 From: anishk85 Date: Sun, 5 Oct 2025 14:42:10 +0530 Subject: [PATCH 3/3] improved code formattings --- .../particle_swarm_optimization.py | 195 +++++++----------- 1 file changed, 75 insertions(+), 120 deletions(-) diff --git a/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py b/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py index 030af7c354..c2e8b6a5f4 100755 --- a/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py +++ b/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py @@ -21,17 +21,13 @@ import matplotlib.patches as patches import signal import sys - # Add show_animation flag for consistency with other planners show_animation = True - def signal_handler(sig, frame): - print('\nExiting...') - plt.close('all') + print("\nExiting...") + plt.close("all") sys.exit(0) - signal.signal(signal.SIGINT, signal_handler) - class Particle: def __init__(self, search_bounds, spawn_bounds): self.search_bounds = search_bounds @@ -41,60 +37,49 @@ def __init__(self, search_bounds, spawn_bounds): self.pbest_position = self.position.copy() self.pbest_value = np.inf self.path = [self.position.copy()] - def update_velocity(self, gbest_pos, w, c1, c2): """Update particle velocity using PSO equation: v = w*v + c1*r1*(pbest - x) + c2*r2*(gbest - x) """ r1 = np.random.rand(2) r2 = np.random.rand(2) - cognitive = c1 * r1 * (self.pbest_position - self.position) social = c2 * r2 * (gbest_pos - self.position) - self.velocity = w * self.velocity + cognitive + social self.velocity = np.clip(self.velocity, -self.max_velocity, self.max_velocity) - def update_position(self): self.position = self.position + self.velocity - # Keep in bounds for i in range(2): - self.position[i] = np.clip(self.position[i], - self.search_bounds[i][0], - self.search_bounds[i][1]) - + self.position[i] = np.clip( + self.position[i], self.search_bounds[i][0], self.search_bounds[i][1] + ) self.path.append(self.position.copy()) - - class PSOSwarm: - def __init__(self, n_particles, max_iter, target, search_bounds, - spawn_bounds, obstacles): + def __init__( + self, n_particles, max_iter, target, search_bounds, spawn_bounds, obstacles + ): self.n_particles = n_particles self.max_iter = max_iter self.target = np.array(target) self.obstacles = obstacles self.search_bounds = search_bounds - # PSO parameters self.w_start = 0.9 # Initial inertia weight - self.w_end = 0.4 # Final inertia weight - self.c1 = 1.5 # Cognitive coefficient - self.c2 = 1.5 # Social coefficient - + self.w_end = 0.4 # Final inertia weight + self.c1 = 1.5 # Cognitive coefficient + self.c2 = 1.5 # Social coefficient # Initialize particles - self.particles = [Particle(search_bounds, spawn_bounds) - for _ in range(n_particles)] - + self.particles = [ + Particle(search_bounds, spawn_bounds) for _ in range(n_particles) + ] self.gbest_position = None self.gbest_value = np.inf self.gbest_path = [] self.iteration = 0 - def fitness(self, pos): """Calculate fitness - distance to target + obstacle penalty""" dist = np.linalg.norm(pos - self.target) - # Obstacle penalty penalty = 0 for ox, oy, r in self.obstacles: @@ -103,147 +88,112 @@ def fitness(self, pos): penalty += 1000 # Inside obstacle elif obs_dist < r + 5: penalty += 50 / (obs_dist - r + 0.1) # Too close - return dist + penalty - def check_collision(self, start, end, obstacle): """Check if path from start to end hits obstacle using line-circle intersection - - Args: + Args: start: Starting position (numpy array) - end: Ending position (numpy array) + end: Ending position (numpy array) obstacle: Tuple (ox, oy, r) representing obstacle center and radius - Returns: bool: True if collision detected, False otherwise """ ox, oy, r = obstacle center = np.array([ox, oy]) - # Vector math for line-circle intersection d = end - start f = start - center - a = np.dot(d, d) - # FIX: Guard against zero-length steps to prevent ZeroDivisionError if a < 1e-10: # Near-zero length step # Check if start point is inside obstacle return np.linalg.norm(f) <= r - b = 2 * np.dot(f, d) c = np.dot(f, f) - r * r - discriminant = b * b - 4 * a * c - if discriminant < 0: return False - # Check if intersection on segment sqrt_discriminant = np.sqrt(discriminant) t1 = (-b - sqrt_discriminant) / (2 * a) t2 = (-b + sqrt_discriminant) / (2 * a) - return (0 <= t1 <= 1) or (0 <= t2 <= 1) - def step(self): """Run one PSO iteration - Returns: bool: True if algorithm should continue, False if completed """ if self.iteration >= self.max_iter: return False - # Update inertia weight (linear decay) - w = self.w_start - (self.w_start - self.w_end) * (self.iteration / self.max_iter) - + w = self.w_start - (self.w_start - self.w_end) * ( + self.iteration / self.max_iter + ) # Evaluate all particles for particle in self.particles: value = self.fitness(particle.position) - # Update personal best if value < particle.pbest_value: particle.pbest_value = value particle.pbest_position = particle.position.copy() - # Update global best if value < self.gbest_value: self.gbest_value = value self.gbest_position = particle.position.copy() - if self.gbest_position is not None: self.gbest_path.append(self.gbest_position.copy()) - # Update particles for particle in self.particles: particle.update_velocity(self.gbest_position, w, self.c1, self.c2) - # Predict next position next_pos = particle.position + particle.velocity - # Check collision collision = False for obs in self.obstacles: if self.check_collision(particle.position, next_pos, obs): collision = True break - if collision: # Reduce velocity if collision detected particle.velocity *= 0.2 - particle.update_position() - self.iteration += 1 if show_animation and self.iteration % 20 == 0: - print(f"Iteration {self.iteration}/{self.max_iter}, Best: {self.gbest_value:.2f}") - + print( + f"Iteration {self.iteration}/{self.max_iter}, Best: {self.gbest_value:.2f}" + ) return True - - def main(): """Run PSO path planning algorithm demonstration. - This function demonstrates PSO-based path planning with the following setup: - 15 particles exploring a (-50,50) x (-50,50) search space - - Start zone: (-45,-35) to (-35,-35) + - Start zone: (-45,-35) to (-35,-35) - Target: (40, 35) - 4 circular obstacles with collision avoidance - Real-time visualization showing particle convergence (if show_animation=True) - Headless mode support for testing (if show_animation=False) - The algorithm runs for up to 150 iterations, displaying particle movement, personal/global best positions, and the evolving optimal path. """ print(__file__ + " start!!") - # Set matplotlib backend for headless environments if not show_animation: - matplotlib.use('Agg') # Use non-GUI backend for testing - + matplotlib.use("Agg") # Use non-GUI backend for testing # Setup parameters N_PARTICLES = 15 MAX_ITER = 150 SEARCH_BOUNDS = [(-50, 50), (-50, 50)] TARGET = [40, 35] SPAWN_AREA = [(-45, -35), (-45, -35)] - OBSTACLES = [ - (10, 15, 8), - (-20, 0, 12), - (20, -25, 10), - (-5, -30, 7) - ] - + OBSTACLES = [(10, 15, 8), (-20, 0, 12), (20, -25, 10), (-5, -30, 7)] swarm = PSOSwarm( n_particles=N_PARTICLES, max_iter=MAX_ITER, target=TARGET, search_bounds=SEARCH_BOUNDS, spawn_bounds=SPAWN_AREA, - obstacles=OBSTACLES + obstacles=OBSTACLES, ) - # pragma: no cover if show_animation: # Visualization setup @@ -252,80 +202,87 @@ def main(): ax.set_ylim(SEARCH_BOUNDS[1]) ax.set_title("PSO Path Planning with Collision Avoidance", fontsize=14) ax.grid(True, alpha=0.3) - # Draw obstacles for ox, oy, r in OBSTACLES: - circle = patches.Circle((ox, oy), r, color='gray', alpha=0.7) + circle = patches.Circle((ox, oy), r, color="gray", alpha=0.7) ax.add_patch(circle) - # Draw spawn area spawn_rect = patches.Rectangle( (SPAWN_AREA[0][0], SPAWN_AREA[1][0]), SPAWN_AREA[0][1] - SPAWN_AREA[0][0], SPAWN_AREA[1][1] - SPAWN_AREA[1][0], - linewidth=2, edgecolor='green', facecolor='green', - alpha=0.2, label='Start Zone' + linewidth=2, + edgecolor="green", + facecolor="green", + alpha=0.2, + label="Start Zone", ) ax.add_patch(spawn_rect) - # Draw target - ax.plot(TARGET[0], TARGET[1], 'r*', markersize=20, label='Target') - + ax.plot(TARGET[0], TARGET[1], "r*", markersize=20, label="Target") # Initialize plot elements - particles_scatter = ax.scatter([], [], c='blue', s=50, alpha=0.6, label='Particles') - gbest_scatter = ax.plot([], [], 'yo', markersize=12, label='Best Position')[0] - particle_paths = [ax.plot([], [], 'b-', lw=0.5, alpha=0.2)[0] for _ in range(N_PARTICLES)] - gbest_path_line = ax.plot([], [], 'y--', lw=2, alpha=0.8, label='Best Path')[0] - - iteration_text = ax.text(0.02, 0.95, '', transform=ax.transAxes, - fontsize=12, verticalalignment='top', - bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.5)) - - ax.legend(loc='upper right') - + particles_scatter = ax.scatter( + [], [], c="blue", s=50, alpha=0.6, label="Particles" + ) + gbest_scatter = ax.plot([], [], "yo", markersize=12, label="Best Position")[0] + particle_paths = [ + ax.plot([], [], "b-", lw=0.5, alpha=0.2)[0] for _ in range(N_PARTICLES) + ] + gbest_path_line = ax.plot([], [], "y--", lw=2, alpha=0.8, label="Best Path")[0] + iteration_text = ax.text( + 0.02, + 0.95, + "", + transform=ax.transAxes, + fontsize=12, + verticalalignment="top", + bbox=dict(boxstyle="round", facecolor="wheat", alpha=0.5), + ) + ax.legend(loc="upper right") def animate(frame): """Animation function for matplotlib FuncAnimation""" if not swarm.step(): - return (particles_scatter, gbest_scatter, gbest_path_line, - iteration_text, *particle_paths) - + return ( + particles_scatter, + gbest_scatter, + gbest_path_line, + iteration_text, + *particle_paths, + ) # Update particle positions positions = np.array([p.position for p in swarm.particles]) particles_scatter.set_offsets(positions) - # Update particle paths for i, particle in enumerate(swarm.particles): if len(particle.path) > 1: path = np.array(particle.path) particle_paths[i].set_data(path[:, 0], path[:, 1]) - # Update global best if swarm.gbest_position is not None: - gbest_scatter.set_data([swarm.gbest_position[0]], - [swarm.gbest_position[1]]) - + gbest_scatter.set_data( + [swarm.gbest_position[0]], [swarm.gbest_position[1]] + ) if len(swarm.gbest_path) > 1: gbest = np.array(swarm.gbest_path) gbest_path_line.set_data(gbest[:, 0], gbest[:, 1]) - # Update text iteration_text.set_text( - f'Iteration: {swarm.iteration}/{MAX_ITER}\n' - f'Best Fitness: {swarm.gbest_value:.2f}' + f"Iteration: {swarm.iteration}/{MAX_ITER}\n" + f"Best Fitness: {swarm.gbest_value:.2f}" + ) + return ( + particles_scatter, + gbest_scatter, + gbest_path_line, + iteration_text, + *particle_paths, ) - - return (particles_scatter, gbest_scatter, gbest_path_line, - iteration_text, *particle_paths) - # Create animation and store reference to prevent garbage collection animation_ref = animation.FuncAnimation( - fig, animate, frames=MAX_ITER, - interval=100, blit=True, repeat=False + fig, animate, frames=MAX_ITER, interval=100, blit=True, repeat=False ) - plt.tight_layout() plt.show() - # Keep reference to prevent garbage collection return animation_ref else: @@ -336,19 +293,17 @@ def animate(frame): iteration_count += 1 if iteration_count >= MAX_ITER: break - print(f"PSO completed after {iteration_count} iterations") print(f"Best fitness: {swarm.gbest_value:.2f}") if swarm.gbest_position is not None: - print(f"Best position: [{swarm.gbest_position[0]:.2f}, {swarm.gbest_position[1]:.2f}]") - + print( + f"Best position: [{swarm.gbest_position[0]:.2f}, {swarm.gbest_position[1]:.2f}]" + ) return None - - if __name__ == "__main__": try: main() except KeyboardInterrupt: print("\nProgram interrupted by user") - plt.close('all') + plt.close("all") sys.exit(0) \ No newline at end of file