rust_robotics_planning/
rrg.rs

1#![allow(dead_code)]
2
3//! RRG (Rapidly-exploring Random Graph) path planning algorithm
4//!
5//! Like RRT\* but keeps ALL collision-free edges within the rewiring radius,
6//! maintaining a graph structure instead of a tree. This gives asymptotic
7//! optimality and stronger graph connectivity for multi-query planning.
8
9use std::cmp::Ordering;
10use std::collections::{BinaryHeap, HashMap};
11
12use rand::Rng;
13
14use rust_robotics_core::{Path2D, PathPlanner, Point2D, RoboticsError};
15
16// ---------------------------------------------------------------------------
17// Public types
18// ---------------------------------------------------------------------------
19
20/// A node in the RRG graph.
21#[derive(Debug, Clone)]
22pub struct RRGNode {
23    pub x: f64,
24    pub y: f64,
25    /// Indices of all neighbours connected by a collision-free edge.
26    pub neighbors: Vec<usize>,
27}
28
29impl RRGNode {
30    fn new(x: f64, y: f64) -> Self {
31        RRGNode {
32            x,
33            y,
34            neighbors: Vec::new(),
35        }
36    }
37}
38
39/// Circular obstacle described by centre and radius.
40#[derive(Debug, Clone)]
41pub struct CircleObstacle {
42    pub x: f64,
43    pub y: f64,
44    pub radius: f64,
45}
46
47impl CircleObstacle {
48    pub fn new(x: f64, y: f64, radius: f64) -> Self {
49        Self { x, y, radius }
50    }
51}
52
53/// Axis-aligned bounding box for the sampling area.
54#[derive(Debug, Clone)]
55pub struct AreaBounds {
56    pub xmin: f64,
57    pub xmax: f64,
58    pub ymin: f64,
59    pub ymax: f64,
60}
61
62impl AreaBounds {
63    pub fn new(xmin: f64, xmax: f64, ymin: f64, ymax: f64) -> Self {
64        Self {
65            xmin,
66            xmax,
67            ymin,
68            ymax,
69        }
70    }
71}
72
73/// Configuration for [`RRGPlanner`].
74#[derive(Debug, Clone)]
75pub struct RRGConfig {
76    /// Maximum extension distance per step \[m\].
77    pub expand_dis: f64,
78    /// Resolution used when checking a path segment for collisions \[m\].
79    pub path_resolution: f64,
80    /// Maximum number of iterations.
81    pub max_iter: usize,
82    /// Robot body radius added to each obstacle \[m\].
83    pub robot_radius: f64,
84    /// Radius within which all collision-free neighbours receive a new edge \[m\].
85    pub rewire_radius: f64,
86}
87
88impl Default for RRGConfig {
89    fn default() -> Self {
90        Self {
91            expand_dis: 3.0,
92            path_resolution: 0.5,
93            max_iter: 500,
94            robot_radius: 0.8,
95            rewire_radius: 5.0,
96        }
97    }
98}
99
100// ---------------------------------------------------------------------------
101// Planner
102// ---------------------------------------------------------------------------
103
104/// RRG planner.
105///
106/// Builds a graph incrementally by connecting every new node to **all**
107/// collision-free neighbours within `rewire_radius`, rather than only the
108/// best-cost parent (as RRT\* does).  Dijkstra is then run on the resulting
109/// graph to retrieve the shortest path.
110pub struct RRGPlanner {
111    config: RRGConfig,
112    obstacles: Vec<CircleObstacle>,
113    rand_area: AreaBounds,
114    /// All nodes accumulated during the last `plan` call.
115    nodes: Vec<RRGNode>,
116}
117
118impl RRGPlanner {
119    /// Create a new planner.
120    pub fn new(obstacles: Vec<CircleObstacle>, rand_area: AreaBounds, config: RRGConfig) -> Self {
121        Self {
122            config,
123            obstacles,
124            rand_area,
125            nodes: Vec::new(),
126        }
127    }
128
129    // ------------------------------------------------------------------
130    // Core algorithm
131    // ------------------------------------------------------------------
132
133    fn plan_internal(&mut self, start: Point2D, goal: Point2D) -> Option<Path2D> {
134        self.nodes.clear();
135        self.nodes.push(RRGNode::new(start.x, start.y)); // index 0
136
137        let mut rng = rand::rng();
138
139        for _ in 0..self.config.max_iter {
140            // Sample a random point inside the area bounds.
141            let rx = rng.random_range(self.rand_area.xmin..=self.rand_area.xmax);
142            let ry = rng.random_range(self.rand_area.ymin..=self.rand_area.ymax);
143
144            // Find nearest existing node.
145            let nearest_idx = self.nearest_index(rx, ry);
146            let (nx, ny) = self.steer(nearest_idx, rx, ry);
147
148            // Collision check for the segment from nearest node to the new point.
149            if !self.segment_collision_free(
150                self.nodes[nearest_idx].x,
151                self.nodes[nearest_idx].y,
152                nx,
153                ny,
154            ) {
155                continue;
156            }
157
158            let new_idx = self.nodes.len();
159            self.nodes.push(RRGNode::new(nx, ny));
160
161            // Connect to ALL collision-free neighbours within rewire_radius.
162            let near = self.nodes_within_radius(new_idx);
163            for ni in near {
164                let (ax, ay) = (self.nodes[new_idx].x, self.nodes[new_idx].y);
165                let (bx, by) = (self.nodes[ni].x, self.nodes[ni].y);
166                if self.segment_collision_free(ax, ay, bx, by) {
167                    // Bidirectional edge.
168                    self.nodes[new_idx].neighbors.push(ni);
169                    self.nodes[ni].neighbors.push(new_idx);
170                }
171            }
172        }
173
174        // Try to attach the goal to the graph.
175        let goal_idx = self.attach_goal(goal.x, goal.y)?;
176
177        // Run Dijkstra from node 0 (start) to goal_idx.
178        self.dijkstra(0, goal_idx, start, goal)
179    }
180
181    /// Steer from node `from_idx` towards `(tx, ty)` by at most `expand_dis`.
182    fn steer(&self, from_idx: usize, tx: f64, ty: f64) -> (f64, f64) {
183        let fx = self.nodes[from_idx].x;
184        let fy = self.nodes[from_idx].y;
185        let dx = tx - fx;
186        let dy = ty - fy;
187        let dist = (dx * dx + dy * dy).sqrt();
188        if dist <= self.config.expand_dis {
189            (tx, ty)
190        } else {
191            let scale = self.config.expand_dis / dist;
192            (fx + dx * scale, fy + dy * scale)
193        }
194    }
195
196    fn nearest_index(&self, x: f64, y: f64) -> usize {
197        self.nodes
198            .iter()
199            .enumerate()
200            .min_by(|(_, a), (_, b)| {
201                let da = (a.x - x).powi(2) + (a.y - y).powi(2);
202                let db = (b.x - x).powi(2) + (b.y - y).powi(2);
203                da.partial_cmp(&db).unwrap_or(Ordering::Equal)
204            })
205            .map(|(i, _)| i)
206            .unwrap_or(0)
207    }
208
209    /// Returns indices of existing nodes (excluding `exclude`) within `rewire_radius`.
210    fn nodes_within_radius(&self, exclude: usize) -> Vec<usize> {
211        let r2 = self.config.rewire_radius.powi(2);
212        let nx = self.nodes[exclude].x;
213        let ny = self.nodes[exclude].y;
214        self.nodes
215            .iter()
216            .enumerate()
217            .filter(|(i, node)| {
218                if *i == exclude {
219                    return false;
220                }
221                let dx = node.x - nx;
222                let dy = node.y - ny;
223                dx * dx + dy * dy <= r2
224            })
225            .map(|(i, _)| i)
226            .collect()
227    }
228
229    /// Check that a straight-line segment is free of obstacles.
230    fn segment_collision_free(&self, x1: f64, y1: f64, x2: f64, y2: f64) -> bool {
231        let dx = x2 - x1;
232        let dy = y2 - y1;
233        let dist = (dx * dx + dy * dy).sqrt();
234        let steps = ((dist / self.config.path_resolution).ceil() as usize).max(1);
235
236        for i in 0..=steps {
237            let t = i as f64 / steps as f64;
238            let px = x1 + t * dx;
239            let py = y1 + t * dy;
240            for obs in &self.obstacles {
241                let ex = px - obs.x;
242                let ey = py - obs.y;
243                let r = obs.radius + self.config.robot_radius;
244                if ex * ex + ey * ey <= r * r {
245                    return false;
246                }
247            }
248        }
249        true
250    }
251
252    /// Try to connect the goal to the graph by finding the nearest node that
253    /// has a collision-free line-of-sight.  Returns the goal node index on
254    /// success.
255    fn attach_goal(&mut self, gx: f64, gy: f64) -> Option<usize> {
256        // Collect candidates sorted by distance.
257        let mut candidates: Vec<(usize, f64)> = self
258            .nodes
259            .iter()
260            .enumerate()
261            .map(|(i, n)| {
262                let dx = n.x - gx;
263                let dy = n.y - gy;
264                (i, dx * dx + dy * dy)
265            })
266            .collect();
267        candidates.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(Ordering::Equal));
268
269        for (idx, _) in candidates {
270            if self.segment_collision_free(self.nodes[idx].x, self.nodes[idx].y, gx, gy) {
271                let goal_idx = self.nodes.len();
272                self.nodes.push(RRGNode::new(gx, gy));
273                // Bidirectional edge.
274                self.nodes[goal_idx].neighbors.push(idx);
275                self.nodes[idx].neighbors.push(goal_idx);
276                return Some(goal_idx);
277            }
278        }
279        None
280    }
281
282    /// Dijkstra shortest path on the graph from `src` to `dst`.
283    fn dijkstra(
284        &self,
285        src: usize,
286        dst: usize,
287        start_pt: Point2D,
288        goal_pt: Point2D,
289    ) -> Option<Path2D> {
290        let n = self.nodes.len();
291        let mut dist = vec![f64::INFINITY; n];
292        let mut prev: HashMap<usize, usize> = HashMap::new();
293        dist[src] = 0.0;
294
295        // Min-heap: (neg_cost, index) — we negate because BinaryHeap is a max-heap.
296        let mut heap: BinaryHeap<DijkstraItem> = BinaryHeap::new();
297        heap.push(DijkstraItem {
298            cost: 0.0,
299            idx: src,
300        });
301
302        while let Some(DijkstraItem { cost, idx }) = heap.pop() {
303            if idx == dst {
304                break;
305            }
306            if cost > dist[idx] {
307                continue;
308            }
309            for &nb in &self.nodes[idx].neighbors {
310                let dx = self.nodes[nb].x - self.nodes[idx].x;
311                let dy = self.nodes[nb].y - self.nodes[idx].y;
312                let edge_cost = (dx * dx + dy * dy).sqrt();
313                let new_cost = dist[idx] + edge_cost;
314                if new_cost < dist[nb] {
315                    dist[nb] = new_cost;
316                    prev.insert(nb, idx);
317                    heap.push(DijkstraItem {
318                        cost: new_cost,
319                        idx: nb,
320                    });
321                }
322            }
323        }
324
325        if dist[dst] == f64::INFINITY {
326            return None;
327        }
328
329        // Reconstruct path.
330        let mut path_pts: Vec<Point2D> = Vec::new();
331        let mut cur = dst;
332        loop {
333            path_pts.push(Point2D::new(self.nodes[cur].x, self.nodes[cur].y));
334            match prev.get(&cur) {
335                Some(&p) => cur = p,
336                None => break,
337            }
338        }
339        path_pts.reverse();
340
341        // Replace first/last with exact start/goal coordinates.
342        if let Some(first) = path_pts.first_mut() {
343            *first = start_pt;
344        }
345        if let Some(last) = path_pts.last_mut() {
346            *last = goal_pt;
347        }
348
349        Some(Path2D::from_points(path_pts))
350    }
351
352    /// Return the graph built during the last planning call (for inspection).
353    pub fn get_graph(&self) -> &[RRGNode] {
354        &self.nodes
355    }
356}
357
358// ---------------------------------------------------------------------------
359// PathPlanner trait
360// ---------------------------------------------------------------------------
361
362impl PathPlanner for RRGPlanner {
363    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
364        // `plan_internal` mutates `self.nodes`, so we need `&mut self`.
365        // We work around the `&self` signature by cloning the planner state.
366        let mut planner = RRGPlanner {
367            config: self.config.clone(),
368            obstacles: self.obstacles.clone(),
369            rand_area: self.rand_area.clone(),
370            nodes: Vec::new(),
371        };
372        planner.plan_internal(start, goal).ok_or_else(|| {
373            RoboticsError::PlanningError("RRG: no path found within max_iter".to_string())
374        })
375    }
376}
377
378// ---------------------------------------------------------------------------
379// Internal priority-queue item for Dijkstra
380// ---------------------------------------------------------------------------
381
382#[derive(Clone)]
383struct DijkstraItem {
384    cost: f64,
385    idx: usize,
386}
387
388impl PartialEq for DijkstraItem {
389    fn eq(&self, other: &Self) -> bool {
390        self.cost == other.cost && self.idx == other.idx
391    }
392}
393
394impl Eq for DijkstraItem {}
395
396impl Ord for DijkstraItem {
397    fn cmp(&self, other: &Self) -> Ordering {
398        // Reverse order so BinaryHeap becomes a min-heap.
399        other
400            .cost
401            .partial_cmp(&self.cost)
402            .unwrap_or(Ordering::Equal)
403    }
404}
405
406impl PartialOrd for DijkstraItem {
407    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
408        Some(self.cmp(other))
409    }
410}
411
412// ---------------------------------------------------------------------------
413// Tests
414// ---------------------------------------------------------------------------
415
416#[cfg(test)]
417mod tests {
418    use super::*;
419
420    fn default_bounds() -> AreaBounds {
421        AreaBounds::new(-2.0, 15.0, -2.0, 15.0)
422    }
423
424    #[test]
425    fn test_rrg_config_defaults() {
426        let cfg = RRGConfig::default();
427        assert_eq!(cfg.expand_dis, 3.0);
428        assert_eq!(cfg.path_resolution, 0.5);
429        assert_eq!(cfg.max_iter, 500);
430        assert_eq!(cfg.robot_radius, 0.8);
431        assert_eq!(cfg.rewire_radius, 5.0);
432    }
433
434    #[test]
435    fn test_rrg_finds_path_no_obstacles() {
436        let cfg = RRGConfig {
437            max_iter: 500,
438            ..Default::default()
439        };
440        let mut planner = RRGPlanner::new(vec![], default_bounds(), cfg);
441
442        let start = Point2D::new(0.0, 0.0);
443        let goal = Point2D::new(6.0, 10.0);
444
445        let path = planner.plan_internal(start, goal);
446        assert!(path.is_some(), "should find a path with no obstacles");
447
448        let path = path.unwrap();
449        let pts = &path.points;
450        assert!(!pts.is_empty());
451
452        let first = pts.first().unwrap();
453        let last = pts.last().unwrap();
454        assert!((first.x - 0.0).abs() < 1e-9 && (first.y - 0.0).abs() < 1e-9);
455        assert!((last.x - 6.0).abs() < 1e-9 && (last.y - 10.0).abs() < 1e-9);
456        println!(
457            "[no_obstacles] path length {} nodes, {} segments",
458            pts.len(),
459            pts.len().saturating_sub(1)
460        );
461    }
462
463    #[test]
464    fn test_rrg_finds_path_with_obstacles() {
465        let obstacles = vec![
466            CircleObstacle::new(5.0, 5.0, 1.0),
467            CircleObstacle::new(3.0, 6.0, 2.0),
468            CircleObstacle::new(3.0, 8.0, 2.0),
469            CircleObstacle::new(3.0, 10.0, 2.0),
470            CircleObstacle::new(7.0, 5.0, 2.0),
471            CircleObstacle::new(9.0, 5.0, 2.0),
472        ];
473        let cfg = RRGConfig {
474            max_iter: 2000,
475            ..Default::default()
476        };
477        let mut planner = RRGPlanner::new(obstacles, default_bounds(), cfg);
478
479        let start = Point2D::new(0.0, 0.0);
480        let goal = Point2D::new(6.0, 10.0);
481
482        let path = planner.plan_internal(start, goal);
483        assert!(path.is_some(), "should find a path even with obstacles");
484
485        let path = path.unwrap();
486        let pts = &path.points;
487        let first = pts.first().unwrap();
488        let last = pts.last().unwrap();
489        assert!((first.x - 0.0).abs() < 1e-9 && (first.y - 0.0).abs() < 1e-9);
490        assert!((last.x - 6.0).abs() < 1e-9 && (last.y - 10.0).abs() < 1e-9);
491        println!("[with_obstacles] path length {} nodes", pts.len());
492    }
493}
rust_robotics_planning/rrg.rs

rust_robotics_planning/
rrg.rs
