rust_robotics_planning/
rrt_sobol.rs

1#![allow(clippy::too_many_arguments)]
2
3//! RRT with Sobol low-discrepancy sampling.
4//!
5//! This mirrors PythonRobotics `PathPlanning/RRT/rrt_with_sobol_sampler.py`
6//! while reusing the existing Rust `RRTPlanner` for steering and collision
7//! handling.
8
9use rand::Rng;
10
11use rust_robotics_core::{Path2D, PathPlanner, Point2D, RoboticsError};
12
13use crate::rrt::{AreaBounds, CircleObstacle, RRTConfig, RRTNode, RRTPlanner};
14
15const SOBOL_BITS: usize = 30;
16const SOBOL_SCALE: f64 = 1.0 / ((1u64 << SOBOL_BITS) as f64);
17
18#[derive(Debug, Clone)]
19struct SobolSequence2D {
20    index: usize,
21    lastq: [u32; 2],
22    directions: [[u32; SOBOL_BITS]; 2],
23}
24
25impl SobolSequence2D {
26    fn new() -> Self {
27        let mut directions = [[0_u32; SOBOL_BITS]; 2];
28        for (column, direction) in directions[0].iter_mut().enumerate() {
29            *direction = 1_u32 << (SOBOL_BITS - column - 1);
30        }
31
32        let mut numerators = [0_u32; SOBOL_BITS];
33        numerators[0] = 1;
34        for column in 1..SOBOL_BITS {
35            numerators[column] = numerators[column - 1] ^ (numerators[column - 1] << 1);
36        }
37        for (column, numerator) in numerators.iter().copied().enumerate() {
38            directions[1][column] = numerator << (SOBOL_BITS - column - 1);
39        }
40
41        Self {
42            index: 0,
43            lastq: [0, 0],
44            directions,
45        }
46    }
47
48    fn next(&mut self) -> [f64; 2] {
49        let point = [
50            self.lastq[0] as f64 * SOBOL_SCALE,
51            self.lastq[1] as f64 * SOBOL_SCALE,
52        ];
53
54        let bit = rightmost_zero_bit_index(self.index);
55        assert!(
56            bit < SOBOL_BITS,
57            "Sobol sequence exceeded supported bit depth of {SOBOL_BITS}"
58        );
59
60        self.lastq[0] ^= self.directions[0][bit];
61        self.lastq[1] ^= self.directions[1][bit];
62        self.index += 1;
63
64        point
65    }
66
67    fn next_in_bounds(&mut self, bounds: &AreaBounds) -> [f64; 2] {
68        let point = self.next();
69        [
70            bounds.xmin + point[0] * (bounds.xmax - bounds.xmin),
71            bounds.ymin + point[1] * (bounds.ymax - bounds.ymin),
72        ]
73    }
74}
75
76fn rightmost_zero_bit_index(mut value: usize) -> usize {
77    let mut bit = 0;
78    loop {
79        if value & 1 == 0 {
80            return bit;
81        }
82        value >>= 1;
83        bit += 1;
84    }
85}
86
87/// RRT planner variant that uses a Sobol low-discrepancy sampler.
88pub struct RRTSobolPlanner {
89    config: RRTConfig,
90    obstacles: Vec<CircleObstacle>,
91    play_area: Option<AreaBounds>,
92    rand_area: AreaBounds,
93    node_list: Vec<RRTNode>,
94}
95
96impl RRTSobolPlanner {
97    pub fn new(
98        obstacles: Vec<CircleObstacle>,
99        rand_area: AreaBounds,
100        play_area: Option<AreaBounds>,
101        config: RRTConfig,
102    ) -> Self {
103        Self {
104            config,
105            obstacles,
106            play_area,
107            rand_area,
108            node_list: Vec::new(),
109        }
110    }
111
112    pub fn from_obstacles(
113        obstacle_list: Vec<(f64, f64, f64)>,
114        rand_area: [f64; 2],
115        expand_dis: f64,
116        path_resolution: f64,
117        goal_sample_rate: i32,
118        max_iter: usize,
119        play_area: Option<[f64; 4]>,
120        robot_radius: f64,
121    ) -> Self {
122        let obstacles = obstacle_list
123            .into_iter()
124            .map(|(x, y, r)| CircleObstacle::new(x, y, r))
125            .collect();
126        let config = RRTConfig {
127            expand_dis,
128            path_resolution,
129            goal_sample_rate,
130            max_iter,
131            robot_radius,
132        };
133        let rand_bounds = AreaBounds::new(rand_area[0], rand_area[1], rand_area[0], rand_area[1]);
134        let play_bounds = play_area.map(AreaBounds::from_array);
135        Self::new(obstacles, rand_bounds, play_bounds, config)
136    }
137
138    pub fn planning(&mut self, start: [f64; 2], goal: [f64; 2]) -> Option<Vec<[f64; 2]>> {
139        let start_pt = Point2D::new(start[0], start[1]);
140        let goal_pt = Point2D::new(goal[0], goal[1]);
141        match self.plan_and_store(start_pt, goal_pt) {
142            Ok(path) => Some(path.points.iter().map(|point| [point.x, point.y]).collect()),
143            Err(_) => None,
144        }
145    }
146
147    pub fn get_tree(&self) -> &[RRTNode] {
148        &self.node_list
149    }
150
151    pub fn get_obstacles(&self) -> &[CircleObstacle] {
152        &self.obstacles
153    }
154
155    fn plan_and_store(&mut self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
156        let mut planner = RRTPlanner::new(
157            self.obstacles.clone(),
158            self.rand_area.clone(),
159            self.play_area.clone(),
160            self.config.clone(),
161        );
162        let goal_sample_rate = self.config.goal_sample_rate;
163        let goal_x = goal.x;
164        let goal_y = goal.y;
165        let rand_area = self.rand_area.clone();
166        let mut sobol = SobolSequence2D::new();
167        let mut rng = rand::rng();
168
169        let path = planner.plan_with_sampler(start, goal, move |_| {
170            if rng.random_range(0..=100) > goal_sample_rate {
171                let sample = sobol.next_in_bounds(&rand_area);
172                RRTNode::new(sample[0], sample[1])
173            } else {
174                RRTNode::new(goal_x, goal_y)
175            }
176        })?;
177
178        self.node_list = planner.get_tree().to_vec();
179        Ok(path)
180    }
181}
182
183impl PathPlanner for RRTSobolPlanner {
184    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
185        let mut planner = RRTSobolPlanner::new(
186            self.obstacles.clone(),
187            self.rand_area.clone(),
188            self.play_area.clone(),
189            self.config.clone(),
190        );
191        planner.plan_and_store(start, goal)
192    }
193}
194
195#[cfg(test)]
196mod tests {
197    use super::*;
198    use std::f64::consts::FRAC_1_SQRT_2;
199
200    fn assert_close(actual: f64, expected: f64) {
201        assert!(
202            (actual - expected).abs() < 1.0e-12,
203            "expected {expected}, got {actual}"
204        );
205    }
206
207    fn assert_point_close(actual: [f64; 2], expected: [f64; 2]) {
208        assert_close(actual[0], expected[0]);
209        assert_close(actual[1], expected[1]);
210    }
211
212    fn create_pythonrobotics_main_planner(goal_sample_rate: i32) -> RRTSobolPlanner {
213        let obstacles = vec![
214            CircleObstacle::new(5.0, 5.0, 1.0),
215            CircleObstacle::new(3.0, 6.0, 2.0),
216            CircleObstacle::new(3.0, 8.0, 2.0),
217            CircleObstacle::new(3.0, 10.0, 2.0),
218            CircleObstacle::new(7.0, 5.0, 2.0),
219            CircleObstacle::new(9.0, 5.0, 2.0),
220            CircleObstacle::new(8.0, 10.0, 1.0),
221        ];
222        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
223        let config = RRTConfig {
224            goal_sample_rate,
225            robot_radius: 0.8,
226            ..Default::default()
227        };
228        RRTSobolPlanner::new(obstacles, rand_area, None, config)
229    }
230
231    #[test]
232    fn test_sobol_sequence_first_points_match_pythonrobotics_reference() {
233        let mut sampler = SobolSequence2D::new();
234        let bounds = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
235        let expected = [
236            [-2.0, -2.0],
237            [6.5, 6.5],
238            [10.75, 2.25],
239            [2.25, 10.75],
240            [4.375, 4.375],
241            [12.875, 12.875],
242            [8.625, 0.125],
243            [0.125, 8.625],
244            [1.1875, 3.3125],
245            [9.6875, 11.8125],
246        ];
247
248        for expected_point in expected {
249            let actual = sampler.next_in_bounds(&bounds);
250            assert_point_close(actual, expected_point);
251        }
252    }
253
254    #[test]
255    fn test_rrt_sobol_goal_sampling_disabled_matches_pythonrobotics_reference() {
256        let mut planner = create_pythonrobotics_main_planner(-1);
257        let path = planner.planning([0.0, 0.0], [6.0, 10.0]).unwrap();
258        let expected_path = [
259            [0.0, 0.0],
260            [2.121_320_343_559_643, 2.121_320_343_559_642_4],
261            [5.120_986_802_026_766, 2.166_054_423_938_105_4],
262            [7.713_273_910_301_528, 0.656_071_792_159_690_4],
263            [10.619_533_093_777_92, -0.088_009_895_868_675],
264            [10.982_945_821_776_296, 2.889_897_286_890_515_3],
265            [12.652_536_566_531_195, 5.382_379_331_967_025],
266            [10.613_696_586_776_955, 7.583_090_929_827_073],
267            [11.146_612_940_692_071, 10.535_378_205_775_984],
268            [12.257_944_072_169_929, 13.321_942_957_617_964],
269            [8.890_625, 13.671_875],
270            [6.898_437_5, 13.273_437_5],
271            [7.695_312_5, 12.476_562_5],
272            [6.699_218_75, 12.144_531_25],
273            [6.0, 10.0],
274        ];
275
276        assert_eq!(planner.get_tree().len(), 86);
277        assert_eq!(path.len(), expected_path.len());
278        for (actual, expected) in path.iter().copied().zip(expected_path.iter().copied()) {
279            assert_point_close(actual, expected);
280        }
281
282        let expected_nodes = [
283            (
284                1,
285                [-2.0, -2.0],
286                Some(0),
287                7,
288                [0.0, -0.353_553_390_593_273_73, -0.707_106_781_186_547_5],
289                [0.0, -0.353_553_390_593_273_8, -FRAC_1_SQRT_2],
290            ),
291            (
292                2,
293                [2.121_320_343_559_643, 2.121_320_343_559_642_4],
294                Some(0),
295                7,
296                [0.0, 0.353_553_390_593_273_8, FRAC_1_SQRT_2],
297                [0.0, 0.353_553_390_593_273_73, FRAC_1_SQRT_2],
298            ),
299            (
300                6,
301                [10.619_533_093_777_92, -0.088_009_895_868_675],
302                Some(4),
303                7,
304                [
305                    7.713_273_910_301_528,
306                    8.197_650_440_880_928,
307                    8.682_026_971_460_326,
308                ],
309                [
310                    0.656_071_792_159_690_4,
311                    0.532_058_177_488_296_1,
312                    0.408_044_562_816_901_9,
313                ],
314            ),
315            (
316                11,
317                [5.968_75, -0.406_25],
318                Some(4),
319                6,
320                [
321                    7.713_273_910_301_528,
322                    7.286_222_224_748_394,
323                    6.859_170_539_195_259,
324                ],
325                [
326                    0.656_071_792_159_690_4,
327                    0.396_020_147_546_112_5,
328                    0.135_968_502_932_534_6,
329                ],
330            ),
331        ];
332
333        for (index, xy, parent, path_len, path_x3, path_y3) in expected_nodes {
334            let node = &planner.get_tree()[index];
335            assert_close(node.x, xy[0]);
336            assert_close(node.y, xy[1]);
337            assert_eq!(node.parent, parent);
338            assert_eq!(node.path_x.len(), path_len);
339            for (actual, expected) in node.path_x.iter().take(3).zip(path_x3.iter()) {
340                assert_close(*actual, *expected);
341            }
342            for (actual, expected) in node.path_y.iter().take(3).zip(path_y3.iter()) {
343                assert_close(*actual, *expected);
344            }
345        }
346    }
347}
rust_robotics_planning/rrt_sobol.rs

rust_robotics_planning/
rrt_sobol.rs
