added pso algorithm

.gitignore

@@ -15,6 +15,7 @@ _build/
 # Distribution / packaging
 .Python
 env/
+venv/
 build/
 develop-eggs/
 dist/

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)

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.