rust_robotics_planning/
particle_swarm_optimization.rs

1//! Particle Swarm Optimization (PSO) path planner
2//!
3//! Treats path planning as an optimization problem where particles explore the
4//! search space to minimize distance to target while avoiding circular obstacles.
5//! Each particle maintains a position, velocity, and personal best; the swarm
6//! collectively converges towards the global best solution.
7//!
8//! Reference: Kennedy & Eberhart (1995), "Particle Swarm Optimization".
9
10use rand::Rng;
11use rust_robotics_core::{Point2D, RoboticsError, RoboticsResult};
12
13/// A circular obstacle defined by centre and radius.
14#[derive(Debug, Clone)]
15pub struct CircleObstacle {
16    pub center: Point2D,
17    pub radius: f64,
18}
19
20impl CircleObstacle {
21    pub fn new(cx: f64, cy: f64, radius: f64) -> Self {
22        Self {
23            center: Point2D::new(cx, cy),
24            radius,
25        }
26    }
27}
28
29/// Axis-aligned rectangular bounds `[min, max]` for each of two dimensions.
30#[derive(Debug, Clone)]
31pub struct Bounds2D {
32    pub x_min: f64,
33    pub x_max: f64,
34    pub y_min: f64,
35    pub y_max: f64,
36}
37
38impl Bounds2D {
39    pub fn new(x_min: f64, x_max: f64, y_min: f64, y_max: f64) -> Self {
40        Self {
41            x_min,
42            x_max,
43            y_min,
44            y_max,
45        }
46    }
47}
48
49/// Configuration for the PSO planner.
50#[derive(Debug, Clone)]
51pub struct PsoConfig {
52    /// Number of particles in the swarm.
53    pub n_particles: usize,
54    /// Maximum number of iterations.
55    pub max_iter: usize,
56    /// Target position.
57    pub target: Point2D,
58    /// Bounds defining the full search space.
59    pub search_bounds: Bounds2D,
60    /// Bounds defining the spawn area for particles.
61    pub spawn_bounds: Bounds2D,
62    /// Circular obstacles.
63    pub obstacles: Vec<CircleObstacle>,
64    /// Initial inertia weight (decays linearly to `w_end`).
65    pub w_start: f64,
66    /// Final inertia weight.
67    pub w_end: f64,
68    /// Cognitive coefficient (pull towards personal best).
69    pub c1: f64,
70    /// Social coefficient (pull towards global best).
71    pub c2: f64,
72    /// Penalty applied when a particle is inside an obstacle.
73    pub inside_obstacle_penalty: f64,
74    /// Proximity penalty factor for being near an obstacle.
75    pub proximity_penalty_factor: f64,
76    /// Proximity distance threshold beyond the obstacle radius.
77    pub proximity_margin: f64,
78}
79
80impl Default for PsoConfig {
81    fn default() -> Self {
82        Self {
83            n_particles: 15,
84            max_iter: 150,
85            target: Point2D::new(40.0, 35.0),
86            search_bounds: Bounds2D::new(-50.0, 50.0, -50.0, 50.0),
87            spawn_bounds: Bounds2D::new(-45.0, -35.0, -45.0, -35.0),
88            obstacles: vec![],
89            w_start: 0.9,
90            w_end: 0.4,
91            c1: 1.5,
92            c2: 1.5,
93            inside_obstacle_penalty: 1000.0,
94            proximity_penalty_factor: 50.0,
95            proximity_margin: 5.0,
96        }
97    }
98}
99
100impl PsoConfig {
101    /// Validate configuration parameters.
102    pub fn validate(&self) -> RoboticsResult<()> {
103        if self.n_particles == 0 {
104            return Err(RoboticsError::InvalidParameter(
105                "n_particles must be > 0".into(),
106            ));
107        }
108        if self.max_iter == 0 {
109            return Err(RoboticsError::InvalidParameter(
110                "max_iter must be > 0".into(),
111            ));
112        }
113        if self.w_start < self.w_end {
114            return Err(RoboticsError::InvalidParameter(
115                "w_start must be >= w_end".into(),
116            ));
117        }
118        Ok(())
119    }
120}
121
122/// A single particle in the swarm.
123#[derive(Debug, Clone)]
124struct Particle {
125    position: [f64; 2],
126    velocity: [f64; 2],
127    personal_best_position: [f64; 2],
128    personal_best_value: f64,
129    max_velocity: [f64; 2],
130    path: Vec<[f64; 2]>,
131}
132
133impl Particle {
134    fn new<R: Rng>(search_bounds: &Bounds2D, spawn_bounds: &Bounds2D, rng: &mut R) -> Self {
135        let position = [
136            rng.random_range(spawn_bounds.x_min..=spawn_bounds.x_max),
137            rng.random_range(spawn_bounds.y_min..=spawn_bounds.y_max),
138        ];
139        let velocity = [rng.random_range(-0.1..0.1), rng.random_range(-0.1..0.1)];
140        let max_velocity = [
141            (search_bounds.x_max - search_bounds.x_min) * 0.05,
142            (search_bounds.y_max - search_bounds.y_min) * 0.05,
143        ];
144        Self {
145            position,
146            velocity,
147            personal_best_position: position,
148            personal_best_value: f64::INFINITY,
149            max_velocity,
150            path: vec![position],
151        }
152    }
153
154    fn update_velocity<R: Rng>(
155        &mut self,
156        gbest_pos: &[f64; 2],
157        w: f64,
158        c1: f64,
159        c2: f64,
160        rng: &mut R,
161    ) {
162        for (i, &gbest) in gbest_pos.iter().enumerate() {
163            let r1: f64 = rng.random();
164            let r2: f64 = rng.random();
165            let cognitive = c1 * r1 * (self.personal_best_position[i] - self.position[i]);
166            let social = c2 * r2 * (gbest - self.position[i]);
167            self.velocity[i] = w * self.velocity[i] + cognitive + social;
168            self.velocity[i] = self.velocity[i].clamp(-self.max_velocity[i], self.max_velocity[i]);
169        }
170    }
171
172    fn update_position(&mut self, search_bounds: &Bounds2D) {
173        self.position[0] =
174            (self.position[0] + self.velocity[0]).clamp(search_bounds.x_min, search_bounds.x_max);
175        self.position[1] =
176            (self.position[1] + self.velocity[1]).clamp(search_bounds.y_min, search_bounds.y_max);
177        self.path.push(self.position);
178    }
179}
180
181/// Result returned by the PSO planner.
182#[derive(Debug, Clone)]
183pub struct PsoResult {
184    /// Best position found by the swarm.
185    pub best_position: Point2D,
186    /// Fitness value at the best position.
187    pub best_fitness: f64,
188    /// Sequence of global-best positions recorded each iteration.
189    pub best_path: Vec<Point2D>,
190    /// Number of iterations actually executed.
191    pub iterations: usize,
192}
193
194/// The PSO path planner.
195#[derive(Debug, Clone)]
196pub struct PsoPlanner {
197    config: PsoConfig,
198}
199
200impl PsoPlanner {
201    pub fn new(config: PsoConfig) -> RoboticsResult<Self> {
202        config.validate()?;
203        Ok(Self { config })
204    }
205
206    /// Run the PSO optimisation and return the result.
207    pub fn plan(&self) -> PsoResult {
208        self.plan_with_rng(&mut rand::rng())
209    }
210
211    /// Run with an explicit RNG (useful for deterministic tests).
212    pub fn plan_with_rng<R: Rng>(&self, rng: &mut R) -> PsoResult {
213        let cfg = &self.config;
214        let mut particles: Vec<Particle> = (0..cfg.n_particles)
215            .map(|_| Particle::new(&cfg.search_bounds, &cfg.spawn_bounds, rng))
216            .collect();
217
218        let mut gbest_position = [0.0_f64; 2];
219        let mut gbest_value = f64::INFINITY;
220        let mut gbest_path: Vec<Point2D> = Vec::new();
221        let mut iterations = 0;
222
223        for iter in 0..cfg.max_iter {
224            let w = cfg.w_start - (cfg.w_start - cfg.w_end) * (iter as f64 / cfg.max_iter as f64);
225
226            // Evaluate fitness and update personal/global best
227            for p in &mut particles {
228                let value = self.fitness(&p.position);
229                if value < p.personal_best_value {
230                    p.personal_best_value = value;
231                    p.personal_best_position = p.position;
232                }
233                if value < gbest_value {
234                    gbest_value = value;
235                    gbest_position = p.position;
236                }
237            }
238            gbest_path.push(Point2D::new(gbest_position[0], gbest_position[1]));
239
240            // Update velocities and positions
241            for p in &mut particles {
242                p.update_velocity(&gbest_position, w, cfg.c1, cfg.c2, rng);
243
244                // Collision check: reduce velocity if path crosses an obstacle
245                let next_pos = [p.position[0] + p.velocity[0], p.position[1] + p.velocity[1]];
246                let collision = cfg
247                    .obstacles
248                    .iter()
249                    .any(|obs| check_line_circle_collision(&p.position, &next_pos, obs));
250                if collision {
251                    p.velocity[0] *= 0.2;
252                    p.velocity[1] *= 0.2;
253                }
254
255                p.update_position(&cfg.search_bounds);
256            }
257            iterations = iter + 1;
258        }
259
260        PsoResult {
261            best_position: Point2D::new(gbest_position[0], gbest_position[1]),
262            best_fitness: gbest_value,
263            best_path: gbest_path,
264            iterations,
265        }
266    }
267
268    /// Fitness function: distance to target + obstacle penalty.
269    fn fitness(&self, pos: &[f64; 2]) -> f64 {
270        let cfg = &self.config;
271        let dx = pos[0] - cfg.target.x;
272        let dy = pos[1] - cfg.target.y;
273        let dist = (dx * dx + dy * dy).sqrt();
274
275        let mut penalty = 0.0;
276        for obs in &cfg.obstacles {
277            let odx = pos[0] - obs.center.x;
278            let ody = pos[1] - obs.center.y;
279            let obs_dist = (odx * odx + ody * ody).sqrt();
280            if obs_dist < obs.radius {
281                penalty += cfg.inside_obstacle_penalty;
282            } else if obs_dist < obs.radius + cfg.proximity_margin {
283                penalty += cfg.proximity_penalty_factor / (obs_dist - obs.radius + 0.1);
284            }
285        }
286        dist + penalty
287    }
288}
289
290/// Check whether a line segment from `start` to `end` intersects a circle obstacle.
291fn check_line_circle_collision(
292    start: &[f64; 2],
293    end: &[f64; 2],
294    obstacle: &CircleObstacle,
295) -> bool {
296    let dx = end[0] - start[0];
297    let dy = end[1] - start[1];
298    let fx = start[0] - obstacle.center.x;
299    let fy = start[1] - obstacle.center.y;
300
301    let a = dx * dx + dy * dy;
302    if a < 1e-10 {
303        // Near-zero length segment: check if start is inside obstacle
304        return (fx * fx + fy * fy).sqrt() <= obstacle.radius;
305    }
306
307    let b = 2.0 * (fx * dx + fy * dy);
308    let c = fx * fx + fy * fy - obstacle.radius * obstacle.radius;
309    let discriminant = b * b - 4.0 * a * c;
310    if discriminant < 0.0 {
311        return false;
312    }
313
314    let sqrt_disc = discriminant.sqrt();
315    let t1 = (-b - sqrt_disc) / (2.0 * a);
316    let t2 = (-b + sqrt_disc) / (2.0 * a);
317    (0.0..=1.0).contains(&t1) || (0.0..=1.0).contains(&t2)
318}
319
320#[cfg(test)]
321mod tests {
322    use super::*;
323
324    fn default_obstacles() -> Vec<CircleObstacle> {
325        vec![
326            CircleObstacle::new(10.0, 15.0, 8.0),
327            CircleObstacle::new(-20.0, 0.0, 12.0),
328            CircleObstacle::new(20.0, -25.0, 10.0),
329            CircleObstacle::new(-5.0, -30.0, 7.0),
330        ]
331    }
332
333    #[test]
334    fn test_pso_converges_toward_target() {
335        let config = PsoConfig {
336            n_particles: 20,
337            max_iter: 200,
338            target: Point2D::new(40.0, 35.0),
339            search_bounds: Bounds2D::new(-50.0, 50.0, -50.0, 50.0),
340            spawn_bounds: Bounds2D::new(-45.0, -35.0, -45.0, -35.0),
341            obstacles: default_obstacles(),
342            ..PsoConfig::default()
343        };
344        let planner = PsoPlanner::new(config).unwrap();
345        let result = planner.plan();
346
347        // The best position should be reasonably close to the target
348        let dist = result.best_position.distance(&Point2D::new(40.0, 35.0));
349        assert!(
350            dist < 15.0,
351            "Expected best position near target, got distance {dist:.2}"
352        );
353        assert_eq!(result.iterations, 200);
354        assert!(!result.best_path.is_empty());
355    }
356
357    #[test]
358    fn test_pso_no_obstacles() {
359        let config = PsoConfig {
360            n_particles: 10,
361            max_iter: 100,
362            target: Point2D::new(0.0, 0.0),
363            search_bounds: Bounds2D::new(-10.0, 10.0, -10.0, 10.0),
364            spawn_bounds: Bounds2D::new(-5.0, -3.0, -5.0, -3.0),
365            obstacles: vec![],
366            ..PsoConfig::default()
367        };
368        let planner = PsoPlanner::new(config).unwrap();
369        let result = planner.plan();
370        let dist = result.best_position.distance(&Point2D::new(0.0, 0.0));
371        assert!(
372            dist < 3.0,
373            "Without obstacles the swarm should converge near the target, dist={dist:.2}"
374        );
375    }
376
377    #[test]
378    fn test_pso_deterministic_with_seed() {
379        let config = PsoConfig {
380            n_particles: 10,
381            max_iter: 50,
382            target: Point2D::new(5.0, 5.0),
383            search_bounds: Bounds2D::new(-10.0, 10.0, -10.0, 10.0),
384            spawn_bounds: Bounds2D::new(-8.0, -6.0, -8.0, -6.0),
385            obstacles: vec![CircleObstacle::new(0.0, 0.0, 3.0)],
386            ..PsoConfig::default()
387        };
388        let planner = PsoPlanner::new(config).unwrap();
389
390        use rand::SeedableRng;
391        let r1 = planner.plan_with_rng(&mut rand::rngs::StdRng::seed_from_u64(42));
392        let r2 = planner.plan_with_rng(&mut rand::rngs::StdRng::seed_from_u64(42));
393        assert_eq!(r1.best_fitness, r2.best_fitness);
394        assert_eq!(r1.best_position.x, r2.best_position.x);
395        assert_eq!(r1.best_position.y, r2.best_position.y);
396    }
397
398    #[test]
399    fn test_line_circle_collision() {
400        let obs = CircleObstacle::new(5.0, 0.0, 2.0);
401        // Line passes through obstacle
402        assert!(check_line_circle_collision(&[0.0, 0.0], &[10.0, 0.0], &obs));
403        // Line misses obstacle
404        assert!(!check_line_circle_collision(
405            &[0.0, 5.0],
406            &[10.0, 5.0],
407            &obs
408        ));
409        // Zero-length segment inside obstacle
410        assert!(check_line_circle_collision(&[5.0, 0.0], &[5.0, 0.0], &obs));
411        // Zero-length segment outside obstacle
412        assert!(!check_line_circle_collision(&[0.0, 0.0], &[0.0, 0.0], &obs));
413    }
414
415    #[test]
416    fn test_fitness_inside_obstacle_penalty() {
417        let config = PsoConfig {
418            obstacles: vec![CircleObstacle::new(0.0, 0.0, 5.0)],
419            target: Point2D::new(10.0, 0.0),
420            ..PsoConfig::default()
421        };
422        let planner = PsoPlanner::new(config).unwrap();
423
424        let fitness_inside = planner.fitness(&[0.0, 0.0]);
425        let fitness_outside = planner.fitness(&[10.0, 0.0]);
426        assert!(
427            fitness_inside > fitness_outside,
428            "Inside-obstacle fitness ({fitness_inside}) should be much larger than at target ({fitness_outside})"
429        );
430    }
431
432    #[test]
433    fn test_invalid_config() {
434        let config = PsoConfig {
435            n_particles: 0,
436            ..PsoConfig::default()
437        };
438        assert!(PsoPlanner::new(config).is_err());
439
440        let config = PsoConfig {
441            max_iter: 0,
442            ..PsoConfig::default()
443        };
444        assert!(PsoPlanner::new(config).is_err());
445    }
446}
rust_robotics_planning/particle_swarm_optimization.rs

rust_robotics_planning/
particle_swarm_optimization.rs
