rust_robotics_planning/
potential_field.rs

1#![allow(dead_code, clippy::too_many_arguments)]
2
3//! Potential Field path planning algorithm
4//!
5//! Uses attractive and repulsive potential fields to guide
6//! the robot from start to goal while avoiding obstacles.
7
8use std::collections::VecDeque;
9
10use rust_robotics_core::{Path2D, Point2D, RoboticsError, RoboticsResult};
11
12// Parameters
13const KP: f64 = 5.0;
14const ETA: f64 = 100.0;
15const AREA_WIDTH: f64 = 30.0;
16const OSCILLATIONS_DETECTION_LENGTH: usize = 3;
17
18pub struct PotentialFieldPlanner {
19    pub resolution: f64,
20    pub robot_radius: f64,
21}
22
23impl PotentialFieldPlanner {
24    pub fn new(resolution: f64, robot_radius: f64) -> Self {
25        PotentialFieldPlanner {
26            resolution,
27            robot_radius,
28        }
29    }
30
31    pub fn planning(
32        &self,
33        sx: f64,
34        sy: f64,
35        gx: f64,
36        gy: f64,
37        ox: &[f64],
38        oy: &[f64],
39    ) -> Option<(Vec<f64>, Vec<f64>)> {
40        let (pmap, minx, miny) = self.calc_potential_field(gx, gy, ox, oy, sx, sy);
41
42        let mut d = ((sx - gx).powi(2) + (sy - gy).powi(2)).sqrt();
43        let mut ix = ((sx - minx) / self.resolution).round() as i32;
44        let mut iy = ((sy - miny) / self.resolution).round() as i32;
45
46        let mut rx = vec![sx];
47        let mut ry = vec![sy];
48        let motion = self.get_motion_model();
49        let mut previous_ids = VecDeque::new();
50
51        while d >= self.resolution {
52            let mut minp = f64::INFINITY;
53            let mut minix = -1;
54            let mut miniy = -1;
55
56            for motion_step in &motion {
57                let inx = ix + motion_step[0];
58                let iny = iy + motion_step[1];
59
60                let p = if inx >= pmap.len() as i32
61                    || iny >= pmap[0].len() as i32
62                    || inx < 0
63                    || iny < 0
64                {
65                    f64::INFINITY
66                } else {
67                    pmap[inx as usize][iny as usize]
68                };
69
70                if minp > p {
71                    minp = p;
72                    minix = inx;
73                    miniy = iny;
74                }
75            }
76
77            ix = minix;
78            iy = miniy;
79            let xp = ix as f64 * self.resolution + minx;
80            let yp = iy as f64 * self.resolution + miny;
81            d = ((gx - xp).powi(2) + (gy - yp).powi(2)).sqrt();
82            rx.push(xp);
83            ry.push(yp);
84
85            if self.oscillations_detection(&mut previous_ids, ix, iy) {
86                break;
87            }
88        }
89
90        Some((rx, ry))
91    }
92
93    fn calc_potential_field(
94        &self,
95        gx: f64,
96        gy: f64,
97        ox: &[f64],
98        oy: &[f64],
99        sx: f64,
100        sy: f64,
101    ) -> (Vec<Vec<f64>>, f64, f64) {
102        let minx = [ox.iter().fold(f64::INFINITY, |a, &b| a.min(b)), sx, gx]
103            .iter()
104            .fold(f64::INFINITY, |a, &b| a.min(b))
105            - AREA_WIDTH / 2.0;
106        let miny = [oy.iter().fold(f64::INFINITY, |a, &b| a.min(b)), sy, gy]
107            .iter()
108            .fold(f64::INFINITY, |a, &b| a.min(b))
109            - AREA_WIDTH / 2.0;
110        let maxx = [ox.iter().fold(f64::NEG_INFINITY, |a, &b| a.max(b)), sx, gx]
111            .iter()
112            .fold(f64::NEG_INFINITY, |a, &b| a.max(b))
113            + AREA_WIDTH / 2.0;
114        let maxy = [oy.iter().fold(f64::NEG_INFINITY, |a, &b| a.max(b)), sy, gy]
115            .iter()
116            .fold(f64::NEG_INFINITY, |a, &b| a.max(b))
117            + AREA_WIDTH / 2.0;
118
119        let xw = ((maxx - minx) / self.resolution).round() as usize;
120        let yw = ((maxy - miny) / self.resolution).round() as usize;
121
122        let mut pmap = vec![vec![0.0; yw]; xw];
123
124        for (ix, column) in pmap.iter_mut().enumerate().take(xw) {
125            let x = ix as f64 * self.resolution + minx;
126            for (iy, value) in column.iter_mut().enumerate().take(yw) {
127                let y = iy as f64 * self.resolution + miny;
128                let ug = self.calc_attractive_potential(x, y, gx, gy);
129                let uo = self.calc_repulsive_potential(x, y, ox, oy);
130                let uf = ug + uo;
131                *value = uf;
132            }
133        }
134
135        (pmap, minx, miny)
136    }
137
138    fn calc_attractive_potential(&self, x: f64, y: f64, gx: f64, gy: f64) -> f64 {
139        0.5 * KP * ((x - gx).powi(2) + (y - gy).powi(2)).sqrt()
140    }
141
142    fn calc_repulsive_potential(&self, x: f64, y: f64, ox: &[f64], oy: &[f64]) -> f64 {
143        let mut dmin = f64::INFINITY;
144        let mut minid = 0;
145
146        for (i, (&ox_i, &oy_i)) in ox.iter().zip(oy.iter()).enumerate() {
147            let d = ((x - ox_i).powi(2) + (y - oy_i).powi(2)).sqrt();
148            if dmin >= d {
149                dmin = d;
150                minid = i;
151            }
152        }
153
154        let dq = ((x - ox[minid]).powi(2) + (y - oy[minid]).powi(2)).sqrt();
155
156        if dq <= self.robot_radius {
157            let dq = if dq <= 0.1 { 0.1 } else { dq };
158            0.5 * ETA * (1.0 / dq - 1.0 / self.robot_radius).powi(2)
159        } else {
160            0.0
161        }
162    }
163
164    fn get_motion_model(&self) -> Vec<[i32; 2]> {
165        vec![
166            [1, 0],
167            [0, 1],
168            [-1, 0],
169            [0, -1],
170            [-1, -1],
171            [-1, 1],
172            [1, -1],
173            [1, 1],
174        ]
175    }
176
177    /// Plan a path from start to goal while avoiding obstacles, returning a [`Path2D`].
178    ///
179    /// This is a convenience wrapper around [`planning()`](Self::planning) that accepts
180    /// [`Point2D`] and returns [`Path2D`].
181    pub fn plan_with_obstacles(
182        &self,
183        start: Point2D,
184        goal: Point2D,
185        ox: &[f64],
186        oy: &[f64],
187    ) -> RoboticsResult<Path2D> {
188        self.planning(start.x, start.y, goal.x, goal.y, ox, oy)
189            .map(|(rx, ry)| Path2D::try_from_xy(&rx, &ry))
190            .transpose()?
191            .ok_or_else(|| {
192                RoboticsError::PlanningError(
193                    "PotentialField: oscillation detected, no path found".to_string(),
194                )
195            })
196    }
197
198    fn oscillations_detection(
199        &self,
200        previous_ids: &mut VecDeque<(i32, i32)>,
201        ix: i32,
202        iy: i32,
203    ) -> bool {
204        previous_ids.push_back((ix, iy));
205
206        if previous_ids.len() > OSCILLATIONS_DETECTION_LENGTH {
207            previous_ids.pop_front();
208        }
209
210        let mut seen = std::collections::HashSet::new();
211        for &index in previous_ids.iter() {
212            if seen.contains(&index) {
213                return true;
214            }
215            seen.insert(index);
216        }
217        false
218    }
219}
220
221#[cfg(test)]
222mod tests {
223    use super::*;
224
225    #[test]
226    fn test_potential_field_creation() {
227        let planner = PotentialFieldPlanner::new(0.5, 5.0);
228        assert_eq!(planner.resolution, 0.5);
229        assert_eq!(planner.robot_radius, 5.0);
230    }
231}
rust_robotics_planning/potential_field.rs

rust_robotics_planning/
potential_field.rs
