rust_robotics_planning/
fmt_star.rs

1//! Fast Marching Tree (FMT*) path planning.
2//!
3//! FMT* is a batch sampling-based planner that expands a wavefront over a
4//! pre-sampled neighborhood graph to build a low-cost collision-free path.
5//!
6//! Reference:
7//! - Lucas Janson, Edward Schmerling, Ashley Clark, Marco Pavone,
8//!   "Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal
9//!   Motion Planning in Many Dimensions":
10//!   <https://stanford.edu/~pavone/papers/Janson.Schmerling.ea.IJRR15.pdf>
11
12use rand::Rng;
13use rust_robotics_core::{Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
14
15/// Circular obstacle represented by center position and radius.
16#[derive(Debug, Clone)]
17pub struct CircleObstacle {
18    pub x: f64,
19    pub y: f64,
20    pub radius: f64,
21}
22
23impl CircleObstacle {
24    /// Creates a circular obstacle.
25    pub fn new(x: f64, y: f64, radius: f64) -> Self {
26        Self { x, y, radius }
27    }
28}
29
30/// Axis-aligned sampling bounds.
31#[derive(Debug, Clone)]
32pub struct AreaBounds {
33    pub xmin: f64,
34    pub xmax: f64,
35    pub ymin: f64,
36    pub ymax: f64,
37}
38
39impl AreaBounds {
40    /// Creates sampling bounds.
41    pub fn new(xmin: f64, xmax: f64, ymin: f64, ymax: f64) -> Self {
42        Self {
43            xmin,
44            xmax,
45            ymin,
46            ymax,
47        }
48    }
49}
50
51/// Configuration for the FMT* planner.
52#[derive(Debug, Clone, Copy)]
53pub struct FMTStarConfig {
54    /// Number of random samples drawn before adding start and goal.
55    pub n_samples: usize,
56    /// Maximum connection distance \[m\].
57    pub connection_radius: f64,
58    /// Robot radius used for obstacle inflation \[m\].
59    pub robot_radius: f64,
60}
61
62impl Default for FMTStarConfig {
63    fn default() -> Self {
64        Self {
65            n_samples: 500,
66            connection_radius: 5.0,
67            robot_radius: 0.8,
68        }
69    }
70}
71
72impl FMTStarConfig {
73    /// Validates the configuration.
74    pub fn validate(&self) -> RoboticsResult<()> {
75        if self.n_samples == 0 {
76            return Err(RoboticsError::InvalidParameter(
77                "FMT*: n_samples must be greater than zero".to_string(),
78            ));
79        }
80        if self.connection_radius <= 0.0 {
81            return Err(RoboticsError::InvalidParameter(
82                "FMT*: connection_radius must be positive".to_string(),
83            ));
84        }
85        if self.robot_radius < 0.0 {
86            return Err(RoboticsError::InvalidParameter(
87                "FMT*: robot_radius cannot be negative".to_string(),
88            ));
89        }
90        Ok(())
91    }
92}
93
94/// Fast Marching Tree planner.
95pub struct FMTStarPlanner {
96    obstacles: Vec<CircleObstacle>,
97    rand_area: AreaBounds,
98    config: FMTStarConfig,
99}
100
101impl FMTStarPlanner {
102    /// Creates a new planner.
103    pub fn new(
104        obstacles: Vec<CircleObstacle>,
105        rand_area: AreaBounds,
106        config: FMTStarConfig,
107    ) -> Self {
108        Self {
109            obstacles,
110            rand_area,
111            config,
112        }
113    }
114
115    fn plan_with_rng<R: Rng + ?Sized>(
116        &self,
117        start: Point2D,
118        goal: Point2D,
119        rng: &mut R,
120    ) -> RoboticsResult<Path2D> {
121        self.config.validate()?;
122        self.validate_query_point(start, "start")?;
123        self.validate_query_point(goal, "goal")?;
124
125        let points = self.sample_points(start, goal, rng);
126        let neighbors = self.build_neighbors(&points);
127        let goal_index = points.len() - 1;
128
129        let mut parents = vec![None; points.len()];
130        let mut costs = vec![f64::INFINITY; points.len()];
131        let mut is_open = vec![false; points.len()];
132        let mut is_unvisited = vec![true; points.len()];
133        let mut open_vertices = vec![0usize];
134
135        costs[0] = 0.0;
136        is_open[0] = true;
137        is_unvisited[0] = false;
138
139        while !open_vertices.is_empty() {
140            let z_pos = open_vertices
141                .iter()
142                .enumerate()
143                .min_by(|(_, lhs), (_, rhs)| costs[**lhs].total_cmp(&costs[**rhs]))
144                .map(|(idx, _)| idx)
145                .expect("open set is non-empty");
146            let z = open_vertices[z_pos];
147
148            if z == goal_index {
149                return Ok(self.extract_path(&points, &parents, goal_index));
150            }
151
152            let mut newly_open = Vec::new();
153            for &x in &neighbors[z] {
154                if !is_unvisited[x] {
155                    continue;
156                }
157
158                let best_parent = neighbors[x]
159                    .iter()
160                    .copied()
161                    .filter(|&candidate| is_open[candidate])
162                    .map(|candidate| {
163                        let candidate_cost =
164                            costs[candidate] + points[candidate].distance(&points[x]);
165                        (candidate, candidate_cost)
166                    })
167                    .min_by(|lhs, rhs| lhs.1.total_cmp(&rhs.1));
168
169                let Some((parent, best_cost)) = best_parent else {
170                    continue;
171                };
172
173                if self.is_segment_collision_free(points[parent], points[x]) {
174                    parents[x] = Some(parent);
175                    costs[x] = best_cost;
176                    newly_open.push(x);
177                }
178            }
179
180            open_vertices.swap_remove(z_pos);
181            is_open[z] = false;
182
183            for idx in newly_open {
184                if is_unvisited[idx] {
185                    is_unvisited[idx] = false;
186                    is_open[idx] = true;
187                    open_vertices.push(idx);
188                }
189            }
190        }
191
192        Err(RoboticsError::PlanningError(
193            "FMT*: Cannot find path with current samples".to_string(),
194        ))
195    }
196
197    fn validate_query_point(&self, point: Point2D, label: &str) -> RoboticsResult<()> {
198        let inside_bounds = point.x >= self.rand_area.xmin
199            && point.x <= self.rand_area.xmax
200            && point.y >= self.rand_area.ymin
201            && point.y <= self.rand_area.ymax;
202        if !inside_bounds {
203            return Err(RoboticsError::InvalidParameter(format!(
204                "FMT*: {label} is outside sampling bounds"
205            )));
206        }
207        if !self.is_state_collision_free(point) {
208            return Err(RoboticsError::InvalidParameter(format!(
209                "FMT*: {label} lies inside an obstacle"
210            )));
211        }
212        Ok(())
213    }
214
215    fn sample_points<R: Rng + ?Sized>(
216        &self,
217        start: Point2D,
218        goal: Point2D,
219        rng: &mut R,
220    ) -> Vec<Point2D> {
221        let mut points = Vec::with_capacity(self.config.n_samples + 2);
222        points.push(start);
223
224        while points.len() < self.config.n_samples + 1 {
225            let point = Point2D::new(
226                rng.random_range(self.rand_area.xmin..=self.rand_area.xmax),
227                rng.random_range(self.rand_area.ymin..=self.rand_area.ymax),
228            );
229            if self.is_state_collision_free(point) {
230                points.push(point);
231            }
232        }
233
234        points.push(goal);
235        points
236    }
237
238    fn build_neighbors(&self, points: &[Point2D]) -> Vec<Vec<usize>> {
239        let mut neighbors = vec![Vec::new(); points.len()];
240        for i in 0..points.len() {
241            for j in (i + 1)..points.len() {
242                if points[i].distance(&points[j]) <= self.config.connection_radius {
243                    neighbors[i].push(j);
244                    neighbors[j].push(i);
245                }
246            }
247        }
248        neighbors
249    }
250
251    fn extract_path(
252        &self,
253        points: &[Point2D],
254        parents: &[Option<usize>],
255        goal_index: usize,
256    ) -> Path2D {
257        let mut path = vec![points[goal_index]];
258        let mut current = goal_index;
259
260        while let Some(parent) = parents[current] {
261            path.push(points[parent]);
262            current = parent;
263        }
264
265        path.reverse();
266        Path2D::from_points(path)
267    }
268
269    fn is_state_collision_free(&self, point: Point2D) -> bool {
270        self.obstacles.iter().all(|obstacle| {
271            let dx = point.x - obstacle.x;
272            let dy = point.y - obstacle.y;
273            (dx * dx + dy * dy).sqrt() > obstacle.radius + self.config.robot_radius
274        })
275    }
276
277    fn is_segment_collision_free(&self, start: Point2D, end: Point2D) -> bool {
278        let distance = start.distance(&end);
279        if distance <= f64::EPSILON {
280            return self.is_state_collision_free(start);
281        }
282
283        let step = (self.config.robot_radius * 0.5).max(0.05);
284        let n_steps = (distance / step).ceil() as usize;
285
286        for i in 0..=n_steps {
287            let t = i as f64 / n_steps as f64;
288            let point = Point2D::new(
289                start.x + (end.x - start.x) * t,
290                start.y + (end.y - start.y) * t,
291            );
292            if !self.is_state_collision_free(point) {
293                return false;
294            }
295        }
296
297        true
298    }
299}
300
301impl PathPlanner for FMTStarPlanner {
302    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
303        let mut rng = rand::rng();
304        self.plan_with_rng(start, goal, &mut rng)
305    }
306}
307
308#[cfg(test)]
309mod tests {
310    use super::*;
311    use rand::{rngs::StdRng, SeedableRng};
312
313    fn path_length(path: &Path2D) -> f64 {
314        path.points
315            .windows(2)
316            .map(|pair| pair[0].distance(&pair[1]))
317            .sum()
318    }
319
320    fn assert_close(actual: f64, expected: f64, tolerance: f64) {
321        assert!(
322            (actual - expected).abs() <= tolerance,
323            "expected {expected:.12}, got {actual:.12}, tolerance {tolerance:.3e}"
324        );
325    }
326
327    #[test]
328    fn test_fmt_star_config_defaults() {
329        let config = FMTStarConfig::default();
330        assert_eq!(config.n_samples, 500);
331        assert_eq!(config.connection_radius, 5.0);
332        assert_eq!(config.robot_radius, 0.8);
333    }
334
335    #[test]
336    fn test_fmt_star_finds_path_without_obstacles() {
337        let planner = FMTStarPlanner::new(
338            Vec::new(),
339            AreaBounds::new(-1.0, 5.0, -1.0, 5.0),
340            FMTStarConfig {
341                n_samples: 50,
342                connection_radius: 10.0,
343                robot_radius: 0.1,
344            },
345        );
346        let mut rng = StdRng::seed_from_u64(7);
347        let path = planner
348            .plan_with_rng(Point2D::new(0.0, 0.0), Point2D::new(4.0, 4.0), &mut rng)
349            .expect("planner should find a path");
350
351        assert!(!path.is_empty());
352        assert_eq!(path.points.first().expect("path").x, 0.0);
353        assert_eq!(path.points.first().expect("path").y, 0.0);
354        assert_eq!(path.points.last().expect("path").x, 4.0);
355        assert_eq!(path.points.last().expect("path").y, 4.0);
356    }
357
358    #[test]
359    fn test_fmt_star_finds_path_with_obstacles() {
360        let planner = FMTStarPlanner::new(
361            vec![CircleObstacle::new(2.5, 2.5, 0.9)],
362            AreaBounds::new(-1.0, 6.0, -1.0, 6.0),
363            FMTStarConfig {
364                n_samples: 400,
365                connection_radius: 2.5,
366                robot_radius: 0.2,
367            },
368        );
369        let mut rng = StdRng::seed_from_u64(19);
370        let path = planner
371            .plan_with_rng(Point2D::new(0.0, 0.0), Point2D::new(5.0, 5.0), &mut rng)
372            .expect("planner should find a path around the obstacle");
373
374        assert!(path.points.len() >= 2);
375        assert_eq!(path.points.first().expect("path").x, 0.0);
376        assert_eq!(path.points.first().expect("path").y, 0.0);
377        assert_eq!(path.points.last().expect("path").x, 5.0);
378        assert_eq!(path.points.last().expect("path").y, 5.0);
379        assert!(path
380            .points
381            .iter()
382            .all(|point| { point.distance(&Point2D::new(2.5, 2.5)) > 1.1 }));
383    }
384
385    #[test]
386    fn test_fmt_star_obstacle_case_regression() {
387        let planner = FMTStarPlanner::new(
388            vec![CircleObstacle::new(2.5, 2.5, 0.9)],
389            AreaBounds::new(-1.0, 6.0, -1.0, 6.0),
390            FMTStarConfig {
391                n_samples: 400,
392                connection_radius: 2.5,
393                robot_radius: 0.2,
394            },
395        );
396        let mut rng = StdRng::seed_from_u64(19);
397        let path = planner
398            .plan_with_rng(Point2D::new(0.0, 0.0), Point2D::new(5.0, 5.0), &mut rng)
399            .expect("planner should find the regression path");
400
401        assert_eq!(path.points.len(), 5);
402        assert_close(path_length(&path), 7.535322534477, 1.0e-9);
403        assert_close(path.points[1].x, 0.726415085720, 1.0e-9);
404        assert_close(path.points[1].y, 1.726691311279, 1.0e-9);
405        assert_close(path.points[2].x, 1.269433984463, 1.0e-9);
406        assert_close(path.points[2].y, 3.074339829122, 1.0e-9);
407        assert_close(path.points[3].x, 3.149556108870, 1.0e-9);
408        assert_close(path.points[3].y, 4.214733441962, 1.0e-9);
409    }
410}
rust_robotics_planning/fmt_star.rs

rust_robotics_planning/
fmt_star.rs
