1use rust_robotics_core::{RoboticsError, RoboticsResult};
10
11use crate::sipp::{DynamicObstacle, Interval, SippConfig, SippPath, SippPlanner};
12
13#[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#[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
64pub 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#[derive(Debug, Clone)]
103pub struct ConformalSippConfig {
104 pub width: i32,
105 pub height: i32,
106 pub obstacle_map: Vec<Vec<bool>>,
108 pub predicted_obstacles: Vec<PredictedObstacleTrajectory>,
110 pub calibration_errors_by_time: Vec<Vec<f64>>,
112 pub time_horizon: u64,
114 pub required_confidence: f64,
116 pub obstacle_radius: f64,
118 pub allow_diagonal: bool,
120}
121
122#[derive(Debug, Clone, PartialEq)]
124pub struct ConformalSippPlan {
125 pub path: SippPath,
126 pub min_confidence: f64,
128 pub trajectory_violation_bound: f64,
131}
132
133pub 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 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 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 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 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}