Skip to main content

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