rust_robotics_planning/
conformal_sipp.rs

1//! Conformal Prediction Safe Interval Path Planning (CP-SIPP).
2//!
3//! This module reproduces the global-planning slice from Liang et al.,
4//! "Time-aware Motion Planning in Dynamic Environments with Conformal
5//! Prediction" (L4DC 2026).  It converts predicted obstacle trajectories plus
6//! calibration nonconformity scores into confidence-filtered safe intervals, then
7//! delegates the compressed interval search to the existing SIPP implementation.
8
9use rust_robotics_core::{RoboticsError, RoboticsResult};
10
11use crate::sipp::{DynamicObstacle, Interval, SippConfig, SippPath, SippPlanner};
12
13/// A predicted obstacle position at a discrete planning time.
14#[derive(Debug, Clone, Copy, PartialEq)]
15pub struct PredictedObstaclePoint {
16    pub t: u64,
17    pub x: f64,
18    pub y: f64,
19}
20
21impl PredictedObstaclePoint {
22    pub fn new(t: u64, x: f64, y: f64) -> Self {
23        Self { t, x, y }
24    }
25}
26
27/// A discrete predicted trajectory for one dynamic obstacle.
28#[derive(Debug, Clone, PartialEq)]
29pub struct PredictedObstacleTrajectory {
30    pub samples: Vec<PredictedObstaclePoint>,
31}
32
33impl PredictedObstacleTrajectory {
34    pub fn new(samples: Vec<PredictedObstaclePoint>) -> Self {
35        Self { samples }
36    }
37
38    fn position_at(&self, t: u64) -> Option<(f64, f64)> {
39        let mut lower: Option<&PredictedObstaclePoint> = None;
40        let mut upper: Option<&PredictedObstaclePoint> = None;
41
42        for sample in &self.samples {
43            if sample.t == t {
44                return Some((sample.x, sample.y));
45            }
46            if sample.t < t && lower.map_or(true, |current| sample.t > current.t) {
47                lower = Some(sample);
48            }
49            if sample.t > t && upper.map_or(true, |current| sample.t < current.t) {
50                upper = Some(sample);
51            }
52        }
53
54        let (lower, upper) = (lower?, upper?);
55        let span = (upper.t - lower.t) as f64;
56        let alpha = (t - lower.t) as f64 / span;
57        Some((
58            lower.x + alpha * (upper.x - lower.x),
59            lower.y + alpha * (upper.y - lower.y),
60        ))
61    }
62}
63
64/// Configuration for CP-SIPP over a discrete grid and finite time horizon.
65///
66/// `calibration_errors_by_time[t]` contains historical nonconformity scores for
67/// prediction horizon `t`. A grid cell is safe at time `t` when the empirical
68/// confidence induced by its distance to the nearest predicted obstacle is at
69/// least `required_confidence`.
70#[derive(Debug, Clone)]
71pub struct ConformalSippConfig {
72    pub width: i32,
73    pub height: i32,
74    /// Static obstacle map: `true` means blocked.
75    pub obstacle_map: Vec<Vec<bool>>,
76    /// Predicted obstacle centers indexed by discrete time.
77    pub predicted_obstacles: Vec<PredictedObstacleTrajectory>,
78    /// Historical prediction errors for each discrete time.
79    pub calibration_errors_by_time: Vec<Vec<f64>>,
80    /// Last discrete time that may appear in a plan.
81    pub time_horizon: u64,
82    /// Required empirical confidence in `[0, 1]`.
83    pub required_confidence: f64,
84    /// Base footprint radius added to prediction error margins.
85    pub obstacle_radius: f64,
86    /// Whether to allow diagonal (8-connected) movement.
87    pub allow_diagonal: bool,
88}
89
90/// A CP-SIPP path plus trajectory-level confidence summary.
91#[derive(Debug, Clone, PartialEq)]
92pub struct ConformalSippPlan {
93    pub path: SippPath,
94    /// Minimum empirical confidence over all returned waypoints.
95    pub min_confidence: f64,
96    /// Boole-union upper bound on violation probability, using waypoint
97    /// confidences: `sum_t (1 - c_t)`, capped at 1.
98    pub trajectory_violation_bound: f64,
99}
100
101/// CP-SIPP planner with a fixed confidence threshold.
102pub struct ConformalSippPlanner {
103    planner: SippPlanner,
104    width: i32,
105    height: i32,
106    predicted_obstacles: Vec<PredictedObstacleTrajectory>,
107    calibration_errors_by_time: Vec<Vec<f64>>,
108    time_horizon: u64,
109    required_confidence: f64,
110    obstacle_radius: f64,
111}
112
113impl ConformalSippPlanner {
114    pub fn new(config: ConformalSippConfig) -> RoboticsResult<Self> {
115        Self::validate_config(&config)?;
116
117        let dynamic_obstacles = Self::build_dynamic_obstacles(&config);
118        let planner = SippPlanner::new(SippConfig {
119            width: config.width,
120            height: config.height,
121            obstacle_map: config.obstacle_map,
122            dynamic_obstacles,
123            allow_diagonal: config.allow_diagonal,
124        })?;
125
126        Ok(Self {
127            planner,
128            width: config.width,
129            height: config.height,
130            predicted_obstacles: config.predicted_obstacles,
131            calibration_errors_by_time: config.calibration_errors_by_time,
132            time_horizon: config.time_horizon,
133            required_confidence: config.required_confidence,
134            obstacle_radius: config.obstacle_radius,
135        })
136    }
137
138    /// Plan a path and report empirical confidence over the returned waypoints.
139    pub fn plan(&self, sx: i32, sy: i32, gx: i32, gy: i32) -> RoboticsResult<ConformalSippPlan> {
140        let path = self.planner.plan(sx, sy, gx, gy)?;
141        let mut min_confidence = 1.0;
142        let mut violation_bound = 0.0;
143
144        for waypoint in &path {
145            let confidence = self.confidence_at(waypoint.x, waypoint.y, waypoint.t)?;
146            if confidence < min_confidence {
147                min_confidence = confidence;
148            }
149            violation_bound += 1.0 - confidence;
150        }
151
152        Ok(ConformalSippPlan {
153            path,
154            min_confidence,
155            trajectory_violation_bound: violation_bound.min(1.0),
156        })
157    }
158
159    /// Empirical conformal confidence that cell `(x, y)` is safe at time `t`.
160    pub fn confidence_at(&self, x: i32, y: i32, t: u64) -> RoboticsResult<f64> {
161        if x < 0 || y < 0 || x >= self.width || y >= self.height {
162            return Err(RoboticsError::InvalidParameter(format!(
163                "cell ({}, {}) is out of bounds",
164                x, y
165            )));
166        }
167        if t > self.time_horizon {
168            return Ok(0.0);
169        }
170        Ok(Self::confidence_from_inputs(
171            &self.predicted_obstacles,
172            &self.calibration_errors_by_time,
173            self.obstacle_radius,
174            x,
175            y,
176            t,
177        ))
178    }
179
180    /// Conservative empirical quantile radius used at time `t`, including the
181    /// configured footprint radius.
182    pub fn conformal_radius_at(&self, t: u64) -> RoboticsResult<f64> {
183        if t > self.time_horizon {
184            return Err(RoboticsError::InvalidParameter(format!(
185                "time {} exceeds horizon {}",
186                t, self.time_horizon
187            )));
188        }
189        let scores = &self.calibration_errors_by_time[t as usize];
190        Ok(Self::empirical_quantile(scores, self.required_confidence) + self.obstacle_radius)
191    }
192
193    /// Get the confidence-filtered safe intervals for a particular cell.
194    pub fn get_safe_intervals(&self, x: i32, y: i32) -> &[Interval] {
195        self.planner.get_safe_intervals(x, y)
196    }
197
198    fn validate_config(config: &ConformalSippConfig) -> RoboticsResult<()> {
199        if config.width <= 0 || config.height <= 0 {
200            return Err(RoboticsError::InvalidParameter(
201                "width and height must be positive".to_string(),
202            ));
203        }
204        if config.time_horizon >= u64::MAX - 1 {
205            return Err(RoboticsError::InvalidParameter(
206                "time_horizon must leave room for half-open intervals".to_string(),
207            ));
208        }
209        if !(0.0..=1.0).contains(&config.required_confidence)
210            || !config.required_confidence.is_finite()
211        {
212            return Err(RoboticsError::InvalidParameter(
213                "required_confidence must be finite and in [0, 1]".to_string(),
214            ));
215        }
216        if config.obstacle_radius < 0.0 || !config.obstacle_radius.is_finite() {
217            return Err(RoboticsError::InvalidParameter(
218                "obstacle_radius must be finite and non-negative".to_string(),
219            ));
220        }
221        if config.obstacle_map.len() != config.width as usize {
222            return Err(RoboticsError::InvalidParameter(format!(
223                "obstacle_map x-dimension ({}) must match width ({})",
224                config.obstacle_map.len(),
225                config.width
226            )));
227        }
228        for col in &config.obstacle_map {
229            if col.len() != config.height as usize {
230                return Err(RoboticsError::InvalidParameter(
231                    "obstacle_map y-dimension must match height".to_string(),
232                ));
233            }
234        }
235
236        let required_score_sets = config.time_horizon as usize + 1;
237        if config.calibration_errors_by_time.len() < required_score_sets {
238            return Err(RoboticsError::InvalidParameter(format!(
239                "calibration_errors_by_time must contain at least {} entries",
240                required_score_sets
241            )));
242        }
243        for (t, scores) in config
244            .calibration_errors_by_time
245            .iter()
246            .take(required_score_sets)
247            .enumerate()
248        {
249            if scores.is_empty() {
250                return Err(RoboticsError::InvalidParameter(format!(
251                    "calibration score set at time {} must not be empty",
252                    t
253                )));
254            }
255            if scores
256                .iter()
257                .any(|score| *score < 0.0 || !score.is_finite())
258            {
259                return Err(RoboticsError::InvalidParameter(format!(
260                    "calibration score set at time {} contains an invalid score",
261                    t
262                )));
263            }
264        }
265
266        for trajectory in &config.predicted_obstacles {
267            for sample in &trajectory.samples {
268                if sample.t > config.time_horizon {
269                    return Err(RoboticsError::InvalidParameter(format!(
270                        "predicted sample time {} exceeds horizon {}",
271                        sample.t, config.time_horizon
272                    )));
273                }
274                if !sample.x.is_finite() || !sample.y.is_finite() {
275                    return Err(RoboticsError::InvalidParameter(
276                        "predicted obstacle samples must be finite".to_string(),
277                    ));
278                }
279            }
280        }
281
282        Ok(())
283    }
284
285    fn build_dynamic_obstacles(config: &ConformalSippConfig) -> Vec<DynamicObstacle> {
286        let mut dynamic_obstacles = Vec::new();
287
288        for y in 0..config.height {
289            for x in 0..config.width {
290                if config.obstacle_map[x as usize][y as usize] {
291                    continue;
292                }
293
294                for t in 0..=config.time_horizon {
295                    let confidence = Self::confidence_from_inputs(
296                        &config.predicted_obstacles,
297                        &config.calibration_errors_by_time,
298                        config.obstacle_radius,
299                        x,
300                        y,
301                        t,
302                    );
303                    if confidence < config.required_confidence {
304                        dynamic_obstacles.push(DynamicObstacle {
305                            x,
306                            y,
307                            interval: Interval::new(t, t + 1),
308                        });
309                    }
310                }
311
312                dynamic_obstacles.push(DynamicObstacle {
313                    x,
314                    y,
315                    interval: Interval::new(config.time_horizon + 1, u64::MAX),
316                });
317            }
318        }
319
320        dynamic_obstacles
321    }
322
323    fn confidence_from_inputs(
324        predicted_obstacles: &[PredictedObstacleTrajectory],
325        calibration_errors_by_time: &[Vec<f64>],
326        obstacle_radius: f64,
327        x: i32,
328        y: i32,
329        t: u64,
330    ) -> f64 {
331        let mut min_distance = f64::INFINITY;
332        for trajectory in predicted_obstacles {
333            if let Some((ox, oy)) = trajectory.position_at(t) {
334                let dx = x as f64 - ox;
335                let dy = y as f64 - oy;
336                min_distance = min_distance.min((dx * dx + dy * dy).sqrt());
337            }
338        }
339
340        if min_distance.is_infinite() {
341            return 1.0;
342        }
343
344        let margin = min_distance - obstacle_radius;
345        if margin < 0.0 {
346            return 0.0;
347        }
348
349        let scores = &calibration_errors_by_time[t as usize];
350        let covered = scores.iter().filter(|score| **score <= margin).count();
351        covered as f64 / scores.len() as f64
352    }
353
354    fn empirical_quantile(scores: &[f64], confidence: f64) -> f64 {
355        let mut sorted = scores.to_vec();
356        sorted.sort_by(|a, b| a.total_cmp(b));
357        let rank = (confidence * sorted.len() as f64).ceil() as usize;
358        let idx = rank.saturating_sub(1).min(sorted.len() - 1);
359        sorted[idx]
360    }
361}
362
363#[cfg(test)]
364mod tests {
365    use super::*;
366
367    fn empty_map(width: i32, height: i32) -> Vec<Vec<bool>> {
368        vec![vec![false; height as usize]; width as usize]
369    }
370
371    fn repeated_scores(horizon: u64, scores: &[f64]) -> Vec<Vec<f64>> {
372        (0..=horizon).map(|_| scores.to_vec()).collect()
373    }
374
375    fn predicted_line(points: &[(u64, f64, f64)]) -> PredictedObstacleTrajectory {
376        PredictedObstacleTrajectory::new(
377            points
378                .iter()
379                .map(|(t, x, y)| PredictedObstaclePoint::new(*t, *x, *y))
380                .collect(),
381        )
382    }
383
384    fn path_length(path: &[crate::sipp::TimedWaypoint]) -> f64 {
385        path.windows(2)
386            .map(|window| {
387                let dx = window[1].x as f64 - window[0].x as f64;
388                let dy = window[1].y as f64 - window[0].y as f64;
389                (dx * dx + dy * dy).sqrt()
390            })
391            .sum()
392    }
393
394    fn off_center_steps(path: &[crate::sipp::TimedWaypoint], center_y: i32) -> usize {
395        path.iter()
396            .filter(|waypoint| waypoint.y != center_y)
397            .count()
398    }
399
400    #[test]
401    fn predicted_trajectory_interpolates_sparse_samples() {
402        let trajectory = predicted_line(&[(0, 0.0, 0.0), (4, 4.0, 2.0)]);
403
404        assert_eq!(trajectory.position_at(0), Some((0.0, 0.0)));
405        assert_eq!(trajectory.position_at(2), Some((2.0, 1.0)));
406        assert_eq!(trajectory.position_at(4), Some((4.0, 2.0)));
407        assert_eq!(trajectory.position_at(5), None);
408    }
409
410    #[test]
411    fn conformal_intervals_use_interpolated_prediction() {
412        let planner = ConformalSippPlanner::new(ConformalSippConfig {
413            width: 5,
414            height: 1,
415            obstacle_map: empty_map(5, 1),
416            predicted_obstacles: vec![predicted_line(&[(0, 0.0, 0.0), (4, 4.0, 0.0)])],
417            calibration_errors_by_time: repeated_scores(4, &[0.1]),
418            time_horizon: 4,
419            required_confidence: 1.0,
420            obstacle_radius: 0.0,
421            allow_diagonal: false,
422        })
423        .unwrap();
424
425        assert_eq!(
426            planner.get_safe_intervals(2, 0),
427            &[Interval::new(0, 2), Interval::new(3, 5)]
428        );
429        assert_eq!(planner.confidence_at(2, 0, 2).unwrap(), 0.0);
430        assert_eq!(planner.confidence_at(2, 0, 0).unwrap(), 1.0);
431    }
432
433    #[test]
434    fn conformal_intervals_expand_with_higher_required_confidence() {
435        let low_confidence = ConformalSippPlanner::new(ConformalSippConfig {
436            width: 3,
437            height: 1,
438            obstacle_map: empty_map(3, 1),
439            predicted_obstacles: vec![predicted_line(&[(2, 1.0, 0.0)])],
440            calibration_errors_by_time: repeated_scores(5, &[0.1, 1.1]),
441            time_horizon: 5,
442            required_confidence: 0.5,
443            obstacle_radius: 0.0,
444            allow_diagonal: false,
445        })
446        .unwrap();
447        let high_confidence = ConformalSippPlanner::new(ConformalSippConfig {
448            width: 3,
449            height: 1,
450            obstacle_map: empty_map(3, 1),
451            predicted_obstacles: vec![predicted_line(&[(2, 1.0, 0.0)])],
452            calibration_errors_by_time: repeated_scores(5, &[0.1, 1.1]),
453            time_horizon: 5,
454            required_confidence: 1.0,
455            obstacle_radius: 0.0,
456            allow_diagonal: false,
457        })
458        .unwrap();
459
460        assert_eq!(
461            low_confidence.get_safe_intervals(0, 0),
462            &[Interval::new(0, 6)]
463        );
464        assert_eq!(
465            high_confidence.get_safe_intervals(0, 0),
466            &[Interval::new(0, 2), Interval::new(3, 6)]
467        );
468    }
469
470    #[test]
471    fn conformal_sipp_avoids_predicted_obstacle_region() {
472        let planner = ConformalSippPlanner::new(ConformalSippConfig {
473            width: 5,
474            height: 3,
475            obstacle_map: empty_map(5, 3),
476            predicted_obstacles: vec![predicted_line(&[
477                (2, 2.0, 1.0),
478                (3, 2.0, 1.0),
479                (4, 2.0, 1.0),
480            ])],
481            calibration_errors_by_time: repeated_scores(10, &[0.25]),
482            time_horizon: 10,
483            required_confidence: 1.0,
484            obstacle_radius: 0.1,
485            allow_diagonal: false,
486        })
487        .unwrap();
488
489        let plan = planner.plan(0, 1, 4, 1).unwrap();
490        assert_eq!(plan.path.first().unwrap().x, 0);
491        assert_eq!(plan.path.last().unwrap().x, 4);
492
493        for waypoint in &plan.path {
494            assert!(
495                !(waypoint.x == 2 && waypoint.y == 1 && (2..5).contains(&waypoint.t)),
496                "path entered conformal obstacle region at {:?}",
497                waypoint
498            );
499        }
500        assert!(plan.min_confidence >= 1.0);
501        assert_eq!(plan.trajectory_violation_bound, 0.0);
502    }
503
504    #[test]
505    fn confidence_sweep_regression_matches_expected_safety_tradeoff() {
506        let width = 9;
507        let height = 5;
508        let start = (0, 2);
509        let goal = (8, 2);
510        let time_horizon = 16;
511        let obstacle_map = empty_map(width, height);
512        let calibration_scores = repeated_scores(time_horizon, &[0.10, 0.60, 1.20]);
513        let expected = [
514            (0.0, 8, 8.0, 0, 0.0, 1.0),
515            (0.5, 10, 10.0, 6, 2.0 / 3.0, 1.0 / 3.0),
516            (0.7, 11, 10.0, 5, 1.0, 0.0),
517            (1.0, 11, 10.0, 5, 1.0, 0.0),
518        ];
519
520        for (required_confidence, arrival_t, length, off_center, min_confidence, violation_bound) in
521            expected
522        {
523            let planner = ConformalSippPlanner::new(ConformalSippConfig {
524                width,
525                height,
526                obstacle_map: obstacle_map.clone(),
527                predicted_obstacles: vec![predicted_line(&[(4, 4.0, 2.0), (5, 4.0, 2.0)])],
528                calibration_errors_by_time: calibration_scores.clone(),
529                time_horizon,
530                required_confidence,
531                obstacle_radius: 0.0,
532                allow_diagonal: false,
533            })
534            .unwrap();
535            let plan = planner.plan(start.0, start.1, goal.0, goal.1).unwrap();
536
537            assert_eq!(plan.path.last().unwrap().t, arrival_t);
538            assert!(
539                (path_length(&plan.path) - length).abs() < 1e-9,
540                "unexpected length for confidence {required_confidence}"
541            );
542            assert_eq!(off_center_steps(&plan.path, start.1), off_center);
543            assert!(
544                (plan.min_confidence - min_confidence).abs() < 1e-9,
545                "unexpected min confidence for confidence {required_confidence}"
546            );
547            assert!(
548                (plan.trajectory_violation_bound - violation_bound).abs() < 1e-9,
549                "unexpected violation bound for confidence {required_confidence}"
550            );
551        }
552    }
553
554    #[test]
555    fn conformal_radius_uses_empirical_quantile_plus_footprint() {
556        let planner = ConformalSippPlanner::new(ConformalSippConfig {
557            width: 2,
558            height: 2,
559            obstacle_map: empty_map(2, 2),
560            predicted_obstacles: vec![],
561            calibration_errors_by_time: repeated_scores(3, &[0.1, 0.4, 0.8, 1.2]),
562            time_horizon: 3,
563            required_confidence: 0.75,
564            obstacle_radius: 0.2,
565            allow_diagonal: false,
566        })
567        .unwrap();
568
569        assert!((planner.conformal_radius_at(0).unwrap() - 1.0).abs() < 1e-9);
570    }
571
572    #[test]
573    fn conformal_config_rejects_missing_calibration_horizon() {
574        let result = ConformalSippPlanner::new(ConformalSippConfig {
575            width: 2,
576            height: 2,
577            obstacle_map: empty_map(2, 2),
578            predicted_obstacles: vec![],
579            calibration_errors_by_time: repeated_scores(2, &[0.1]),
580            time_horizon: 3,
581            required_confidence: 0.9,
582            obstacle_radius: 0.0,
583            allow_diagonal: false,
584        });
585
586        assert!(matches!(result, Err(RoboticsError::InvalidParameter(_))));
587    }
588}
rust_robotics_planning/conformal_sipp.rs

rust_robotics_planning/
conformal_sipp.rs
