Skip to main content

rust_robotics_control/
meta_control.rs

1//! Meta-Control: deterministic controller mode selection.
2//!
3//! A mode selector drives the shared Controller Arena engine through a
4//! [`PathTracker`] facade and switches between Pure Pursuit, Stanley, and LQR
5//! Steer mid-run according to a deterministic policy. Each sub-controller
6//! keeps its own internal state, so a switch resumes the selected controller
7//! exactly where its own history left off — the selector is evaluated against
8//! the same shared simulation clock, speed model, and turn-rate response as the
9//! arena's fixed controllers.
10//!
11//! This is the differentiator the repository can show that few others can: all
12//! controllers live under one `PathTracker` contract, so a *meta* controller
13//! that composes them is just another implementor of the same trait.
14
15use crate::controller_arena::{
16    apply_shared_response, build_tracker, cross_track_error, invalid, metrics_from_samples,
17    normalize_angle, propagate, state_is_finite, ArenaControllerKind, ArenaMetrics, ArenaSample,
18    ArenaScenario,
19};
20use rust_robotics_core::{
21    ControlInput, Path2D, PathTracker, Point2D, RoboticsError, RoboticsResult, State2D,
22};
23
24/// Deterministic rule that picks which controller drives the current step.
25#[derive(Debug, Clone, Copy, PartialEq)]
26pub enum MetaControlPolicy {
27    /// Never switch: reproduces a single fixed controller (baseline).
28    Fixed(ArenaControllerKind),
29    /// Use `error_mode` while the cross-track error exceeds the threshold,
30    /// otherwise `cruise_mode`.
31    SwitchOnError {
32        error_threshold: f64,
33        error_mode: ArenaControllerKind,
34        cruise_mode: ArenaControllerKind,
35    },
36    /// Use `tight_mode` while the local path curvature exceeds the threshold,
37    /// otherwise `smooth_mode`.
38    SwitchOnCurvature {
39        curvature_threshold: f64,
40        tight_mode: ArenaControllerKind,
41        smooth_mode: ArenaControllerKind,
42    },
43}
44
45impl MetaControlPolicy {
46    pub fn label(self) -> &'static str {
47        match self {
48            MetaControlPolicy::Fixed(kind) => match kind {
49                ArenaControllerKind::PurePursuit => "fixed: pure pursuit",
50                ArenaControllerKind::Stanley => "fixed: stanley",
51                ArenaControllerKind::LqrSteer => "fixed: lqr steer",
52            },
53            MetaControlPolicy::SwitchOnError { .. } => "meta: switch on error",
54            MetaControlPolicy::SwitchOnCurvature { .. } => "meta: switch on curvature",
55        }
56    }
57
58    pub fn validate(self) -> RoboticsResult<()> {
59        match self {
60            MetaControlPolicy::Fixed(_) => Ok(()),
61            MetaControlPolicy::SwitchOnError {
62                error_threshold, ..
63            } => {
64                if !error_threshold.is_finite() || error_threshold <= 0.0 {
65                    Err(invalid(
66                        "switch-on-error threshold must be finite and positive",
67                    ))
68                } else {
69                    Ok(())
70                }
71            }
72            MetaControlPolicy::SwitchOnCurvature {
73                curvature_threshold,
74                ..
75            } => {
76                if !curvature_threshold.is_finite() || curvature_threshold <= 0.0 {
77                    Err(invalid(
78                        "switch-on-curvature threshold must be finite and positive",
79                    ))
80                } else {
81                    Ok(())
82                }
83            }
84        }
85    }
86}
87
88/// A [`PathTracker`] that composes the three arena controllers and selects one
89/// per step. Holds one tracker instance per controller kind so internal state
90/// survives mode switches.
91pub struct MetaController {
92    policy: MetaControlPolicy,
93    goal_tolerance: f64,
94    trackers: [Box<dyn PathTracker>; 3],
95    active: Option<ArenaControllerKind>,
96}
97
98impl MetaController {
99    pub fn new(scenario: &ArenaScenario, policy: MetaControlPolicy) -> RoboticsResult<Self> {
100        policy.validate()?;
101        scenario.validate()?;
102        Ok(Self {
103            policy,
104            goal_tolerance: scenario.goal_tolerance,
105            trackers: ArenaControllerKind::ALL.map(|kind| build_tracker(kind, scenario)),
106            active: None,
107        })
108    }
109
110    pub fn policy(&self) -> MetaControlPolicy {
111        self.policy
112    }
113
114    /// The mode chosen for the most recent [`PathTracker::compute_control`]
115    /// call, if any.
116    pub fn active_mode(&self) -> Option<ArenaControllerKind> {
117        self.active
118    }
119
120    /// Pure selection logic, exposed so callers can record or test the policy
121    /// without advancing any controller state.
122    pub fn select_mode(&self, state: &State2D, path: &Path2D) -> ArenaControllerKind {
123        match self.policy {
124            MetaControlPolicy::Fixed(kind) => kind,
125            MetaControlPolicy::SwitchOnError {
126                error_threshold,
127                error_mode,
128                cruise_mode,
129            } => {
130                let error = cross_track_error(path, *state);
131                if error > error_threshold {
132                    error_mode
133                } else {
134                    cruise_mode
135                }
136            }
137            MetaControlPolicy::SwitchOnCurvature {
138                curvature_threshold,
139                tight_mode,
140                smooth_mode,
141            } => {
142                let curvature = path_curvature_at(path, state);
143                if curvature > curvature_threshold {
144                    tight_mode
145                } else {
146                    smooth_mode
147                }
148            }
149        }
150    }
151
152    fn tracker_for(&mut self, kind: ArenaControllerKind) -> &mut Box<dyn PathTracker> {
153        &mut self.trackers[kind_index(kind)]
154    }
155}
156
157impl PathTracker for MetaController {
158    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
159        let kind = self.select_mode(current_state, path);
160        self.active = Some(kind);
161        self.tracker_for(kind).compute_control(current_state, path)
162    }
163
164    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
165        current_state.position().distance(&goal) < self.goal_tolerance
166    }
167}
168
169/// One meta-control simulation sample. Identical to [`ArenaSample`] plus the
170/// controller mode that produced the command.
171#[derive(Debug, Clone, Copy, PartialEq)]
172pub struct MetaControlSample {
173    pub time: f64,
174    pub state: State2D,
175    pub command: ControlInput,
176    pub cross_track_error: f64,
177    pub active_mode: ArenaControllerKind,
178}
179
180/// Complete deterministic trace for one meta-control policy.
181#[derive(Debug, Clone, PartialEq)]
182pub struct MetaControlRun {
183    pub policy: MetaControlPolicy,
184    pub samples: Vec<MetaControlSample>,
185    pub metrics: ArenaMetrics,
186    pub switch_count: usize,
187}
188
189impl MetaControlRun {
190    /// Number of samples spent in each controller mode, indexed by
191    /// [`ArenaControllerKind`] order.
192    pub fn mode_counts(&self) -> [usize; 3] {
193        let mut counts = [0usize; 3];
194        for sample in &self.samples {
195            counts[kind_index(sample.active_mode)] += 1;
196        }
197        counts
198    }
199
200    /// Fraction of samples spent in `mode`.
201    pub fn mode_share(&self, mode: ArenaControllerKind) -> f64 {
202        let total = self.samples.len().max(1) as f64;
203        let used = self
204            .samples
205            .iter()
206            .filter(|sample| sample.active_mode == mode)
207            .count() as f64;
208        used / total
209    }
210}
211
212/// Runs one meta-control policy under a shared arena scenario.
213pub fn run_meta_control(
214    scenario: &ArenaScenario,
215    policy: MetaControlPolicy,
216) -> RoboticsResult<MetaControlRun> {
217    policy.validate()?;
218    scenario.validate()?;
219    let goal = *scenario
220        .path
221        .points
222        .last()
223        .ok_or_else(|| invalid("meta-control path must contain at least two points"))?;
224
225    let mut controller = MetaController::new(scenario, policy)?;
226    let mut state = scenario.initial_state;
227    let initial_mode = controller.select_mode(&state, &scenario.path);
228    let mut samples = Vec::with_capacity(scenario.max_steps + 1);
229    samples.push(MetaControlSample {
230        time: 0.0,
231        state,
232        command: ControlInput::zero(),
233        cross_track_error: cross_track_error(&scenario.path, state),
234        active_mode: initial_mode,
235    });
236
237    let mut switches = 0usize;
238    let mut previous_mode: Option<ArenaControllerKind> = None;
239    for step in 1..=scenario.max_steps {
240        let requested = controller.compute_control(&state, &scenario.path);
241        let mode = controller
242            .active_mode()
243            .expect("compute_control always selects a mode");
244        if !requested.v.is_finite() || !requested.omega.is_finite() {
245            return Err(RoboticsError::NumericalError(format!(
246                "{} returned a non-finite command",
247                mode.label()
248            )));
249        }
250
251        if let Some(previous) = previous_mode {
252            if previous != mode {
253                switches += 1;
254            }
255        }
256        previous_mode = Some(mode);
257
258        let command = apply_shared_response(scenario, state, requested);
259        state = propagate(state, command, scenario.dt);
260        let sample = MetaControlSample {
261            time: step as f64 * scenario.dt,
262            state,
263            command,
264            cross_track_error: cross_track_error(&scenario.path, state),
265            active_mode: mode,
266        };
267        if !meta_sample_is_finite(&sample) {
268            return Err(RoboticsError::NumericalError(format!(
269                "{} produced a non-finite state",
270                mode.label()
271            )));
272        }
273        samples.push(sample);
274
275        if state.position().distance(&goal) <= scenario.goal_tolerance {
276            break;
277        }
278    }
279
280    let metrics = metrics_from_samples(&to_arena_samples(&samples), goal)?;
281    Ok(MetaControlRun {
282        policy,
283        samples,
284        metrics,
285        switch_count: switches,
286    })
287}
288
289/// Recomputes metrics from meta-control samples (same source of truth as the
290/// arena).
291pub fn meta_metrics_from_samples(
292    samples: &[MetaControlSample],
293    goal: Point2D,
294) -> RoboticsResult<ArenaMetrics> {
295    metrics_from_samples(&to_arena_samples(samples), goal)
296}
297
298fn to_arena_samples(samples: &[MetaControlSample]) -> Vec<ArenaSample> {
299    samples
300        .iter()
301        .map(|sample| ArenaSample {
302            time: sample.time,
303            state: sample.state,
304            command: sample.command,
305            cross_track_error: sample.cross_track_error,
306        })
307        .collect()
308}
309
310fn kind_index(kind: ArenaControllerKind) -> usize {
311    match kind {
312        ArenaControllerKind::PurePursuit => 0,
313        ArenaControllerKind::Stanley => 1,
314        ArenaControllerKind::LqrSteer => 2,
315    }
316}
317
318fn meta_sample_is_finite(sample: &MetaControlSample) -> bool {
319    sample.time.is_finite()
320        && state_is_finite(sample.state)
321        && sample.command.v.is_finite()
322        && sample.command.omega.is_finite()
323        && sample.cross_track_error.is_finite()
324}
325
326/// Magnitude of local path curvature (rad/m) at the robot's nearest path point.
327fn path_curvature_at(path: &Path2D, state: &State2D) -> f64 {
328    let Some(index) = path.nearest_point_index(state.position()) else {
329        return 0.0;
330    };
331    let yaw = path.yaw_profile();
332    let n = yaw.len();
333    if n < 3 {
334        return 0.0;
335    }
336    let i = index.clamp(1, n - 2);
337    let d1 = normalize_angle(yaw[i + 1] - yaw[i]);
338    let d2 = normalize_angle(yaw[i] - yaw[i - 1]);
339    let ds = path.points[i].distance(&path.points[i - 1]).max(1e-3);
340    (d1.abs() + d2.abs()).min(core::f64::consts::PI) / (2.0 * ds)
341}
342
343#[cfg(test)]
344mod tests {
345    use super::*;
346
347    const TOLERANCE: f64 = 1e-12;
348
349    fn assert_metrics_close(left: ArenaMetrics, right: ArenaMetrics) {
350        assert!((left.cross_track_rmse - right.cross_track_rmse).abs() <= TOLERANCE);
351        assert!((left.final_goal_distance - right.final_goal_distance).abs() <= TOLERANCE);
352        assert!((left.max_cross_track_error - right.max_cross_track_error).abs() <= TOLERANCE);
353        assert!(
354            (left.angular_command_smoothness - right.angular_command_smoothness).abs() <= TOLERANCE
355        );
356    }
357
358    #[test]
359    fn fixed_policy_reproduces_arena_run() {
360        for preset in crate::controller_arena::ArenaPreset::ALL {
361            let scenario = preset.scenario(3.0, 0.85);
362            for kind in ArenaControllerKind::ALL {
363                let arena_run = crate::run_controller_arena(&scenario, &[kind])
364                    .unwrap()
365                    .pop()
366                    .unwrap();
367                let meta_run = run_meta_control(&scenario, MetaControlPolicy::Fixed(kind)).unwrap();
368                assert_eq!(meta_run.samples.len(), arena_run.samples.len());
369                for (meta, arena) in meta_run.samples.iter().zip(arena_run.samples.iter()) {
370                    assert_eq!(meta.state, arena.state);
371                    assert_eq!(meta.command, arena.command);
372                    assert!((meta.cross_track_error - arena.cross_track_error).abs() <= TOLERANCE);
373                }
374                assert_metrics_close(meta_run.metrics, arena_run.metrics);
375                assert_eq!(meta_run.switch_count, 0);
376            }
377        }
378    }
379
380    #[test]
381    fn error_policy_switches_to_correction_mode_then_cruises() {
382        let scenario = crate::controller_arena::ArenaPreset::StraightRecovery.scenario(3.0, 0.85);
383        let policy = MetaControlPolicy::SwitchOnError {
384            error_threshold: 0.5,
385            error_mode: ArenaControllerKind::Stanley,
386            cruise_mode: ArenaControllerKind::PurePursuit,
387        };
388        let run = run_meta_control(&scenario, policy).unwrap();
389        // The preset starts ~2.5 m off the path, so Stanley (error_mode) must be
390        // active first, then the selector switches to Pure Pursuit (cruise_mode)
391        // once the cross-track error is under the threshold.
392        assert_eq!(run.samples[0].active_mode, ArenaControllerKind::Stanley);
393        assert!(run.mode_counts()[1] > 0, "Stanley never used");
394        assert!(run.mode_counts()[0] > 0, "Pure Pursuit never used");
395        assert!(run.switch_count >= 1);
396        assert!(run.samples.last().unwrap().cross_track_error.abs() < 0.5);
397    }
398
399    #[test]
400    fn curvature_policy_uses_tight_mode_on_hairpin_arc() {
401        let scenario = crate::controller_arena::ArenaPreset::HairpinRecovery.scenario(3.0, 0.85);
402        let policy = MetaControlPolicy::SwitchOnCurvature {
403            curvature_threshold: 0.10,
404            tight_mode: ArenaControllerKind::LqrSteer,
405            smooth_mode: ArenaControllerKind::PurePursuit,
406        };
407        let run = run_meta_control(&scenario, policy).unwrap();
408        // Hairpin arc radius is 6 m -> ~0.17 rad/m, above the 0.10 threshold;
409        // straights are ~0, so both modes must appear.
410        assert!(run.mode_counts()[2] > 0, "LQR Steer never used on the arc");
411        assert!(
412            run.mode_counts()[0] > 0,
413            "Pure Pursuit never used on straights"
414        );
415        assert!(run.switch_count >= 1);
416    }
417
418    #[test]
419    fn repeated_runs_are_deterministic() {
420        let scenario = crate::controller_arena::ArenaPreset::SlalomRecovery.scenario(2.75, 0.72);
421        let policy = MetaControlPolicy::SwitchOnError {
422            error_threshold: 0.4,
423            error_mode: ArenaControllerKind::Stanley,
424            cruise_mode: ArenaControllerKind::PurePursuit,
425        };
426        let first = run_meta_control(&scenario, policy).unwrap();
427        let second = run_meta_control(&scenario, policy).unwrap();
428        assert_eq!(first, second);
429    }
430
431    #[test]
432    fn invalid_policies_are_rejected() {
433        let scenario = crate::controller_arena::ArenaPreset::StraightRecovery.scenario(3.0, 1.0);
434        assert!(run_meta_control(
435            &scenario,
436            MetaControlPolicy::SwitchOnError {
437                error_threshold: 0.0,
438                error_mode: ArenaControllerKind::Stanley,
439                cruise_mode: ArenaControllerKind::PurePursuit,
440            }
441        )
442        .is_err());
443        assert!(run_meta_control(
444            &scenario,
445            MetaControlPolicy::SwitchOnCurvature {
446                curvature_threshold: f64::NAN,
447                tight_mode: ArenaControllerKind::LqrSteer,
448                smooth_mode: ArenaControllerKind::PurePursuit,
449            }
450        )
451        .is_err());
452    }
453
454    #[test]
455    fn switch_count_matches_adjacent_mode_changes() {
456        let scenario = crate::controller_arena::ArenaPreset::SlalomRecovery.scenario(3.0, 0.85);
457        let policy = MetaControlPolicy::SwitchOnCurvature {
458            curvature_threshold: 0.05,
459            tight_mode: ArenaControllerKind::Stanley,
460            smooth_mode: ArenaControllerKind::LqrSteer,
461        };
462        let run = run_meta_control(&scenario, policy).unwrap();
463        let adjacent = run
464            .samples
465            .windows(2)
466            .filter(|pair| pair[0].active_mode != pair[1].active_mode)
467            .count();
468        assert_eq!(run.switch_count, adjacent);
469    }
470
471    #[test]
472    fn stored_metrics_match_recomputation() {
473        let scenario = crate::controller_arena::ArenaPreset::StraightRecovery.scenario(3.5, 1.0);
474        let goal = *scenario.path.points.last().unwrap();
475        let policy = MetaControlPolicy::SwitchOnError {
476            error_threshold: 0.5,
477            error_mode: ArenaControllerKind::Stanley,
478            cruise_mode: ArenaControllerKind::PurePursuit,
479        };
480        let run = run_meta_control(&scenario, policy).unwrap();
481        let recomputed = meta_metrics_from_samples(&run.samples, goal).unwrap();
482        assert_metrics_close(run.metrics, recomputed);
483    }
484}