Skip to main content

rust_robotics_control/
controller_arena.rs

1//! Deterministic, headless comparison of classical path-tracking controllers.
2//!
3//! The arena deliberately separates simulation from visualization. Every
4//! controller receives the same path, initial state, clock, speed response, and
5//! turn-rate response. The returned samples are suitable for native tests,
6//! browser replay, and future benchmark inputs.
7
8use rust_robotics_core::{
9    ControlInput, Path2D, PathTracker, Point2D, RoboticsError, RoboticsResult, State2D,
10};
11
12use crate::{
13    LQRSteerConfig, LQRSteerController, PurePursuitConfig, PurePursuitController, StanleyConfig,
14    StanleyController,
15};
16
17const DEFAULT_DT: f64 = 0.1;
18const DEFAULT_MAX_LINEAR_SPEED: f64 = 8.0;
19const DEFAULT_MAX_ANGULAR_SPEED: f64 = 1.4;
20const DEFAULT_GOAL_TOLERANCE: f64 = 1.0;
21const SPEED_RESPONSE_PER_SECOND: f64 = 1.8;
22const MIN_CURVATURE_SPEED: f64 = 1e-6;
23/// A run that crosses the line through the goal this close to it \[m\] is
24/// over, even outside the goal tolerance.
25const END_LINE_RADIUS: f64 = 5.0;
26
27/// Controller implementations supported by Controller Arena version 1.
28#[derive(Debug, Clone, Copy, PartialEq, Eq)]
29pub enum ArenaControllerKind {
30    PurePursuit,
31    Stanley,
32    LqrSteer,
33}
34
35impl ArenaControllerKind {
36    pub const ALL: [Self; 3] = [Self::PurePursuit, Self::Stanley, Self::LqrSteer];
37
38    pub fn label(self) -> &'static str {
39        match self {
40            Self::PurePursuit => "Pure Pursuit",
41            Self::Stanley => "Stanley",
42            Self::LqrSteer => "LQR Steer",
43        }
44    }
45}
46
47/// Built-in deterministic paths.
48#[derive(Debug, Clone, Copy, PartialEq, Eq)]
49pub enum ArenaPreset {
50    StraightRecovery,
51    SlalomRecovery,
52    HairpinRecovery,
53}
54
55impl ArenaPreset {
56    pub const ALL: [Self; 3] = [
57        Self::StraightRecovery,
58        Self::SlalomRecovery,
59        Self::HairpinRecovery,
60    ];
61
62    pub fn label(self) -> &'static str {
63        match self {
64            Self::StraightRecovery => "Straight recovery",
65            Self::SlalomRecovery => "Slalom recovery",
66            Self::HairpinRecovery => "Hairpin recovery",
67        }
68    }
69
70    pub fn slug(self) -> &'static str {
71        match self {
72            Self::StraightRecovery => "straight",
73            Self::SlalomRecovery => "slalom",
74            Self::HairpinRecovery => "hairpin",
75        }
76    }
77
78    pub fn from_slug(value: &str) -> Option<Self> {
79        match value {
80            "straight" => Some(Self::StraightRecovery),
81            "slalom" => Some(Self::SlalomRecovery),
82            "hairpin" => Some(Self::HairpinRecovery),
83            _ => None,
84        }
85    }
86
87    /// Creates this preset with user-adjustable common dynamics.
88    pub fn scenario(self, target_speed: f64, turn_rate_response_gain: f64) -> ArenaScenario {
89        let (path, max_steps) = match self {
90            Self::StraightRecovery => (straight_path(), 220),
91            Self::SlalomRecovery => (slalom_path(), 300),
92            Self::HairpinRecovery => (hairpin_path(), 420),
93        };
94        let start = path.points[0];
95        let start_yaw = path.yaw_profile().first().copied().unwrap_or(0.0);
96        ArenaScenario {
97            preset: self,
98            path,
99            initial_state: State2D::new(
100                start.x - 2.0 * start_yaw.cos() - 1.5 * start_yaw.sin(),
101                start.y - 2.0 * start_yaw.sin() + 1.5 * start_yaw.cos(),
102                start_yaw + 0.12,
103                0.5,
104            ),
105            dt: DEFAULT_DT,
106            target_speed,
107            turn_rate_response_gain,
108            max_steps,
109            max_linear_speed: DEFAULT_MAX_LINEAR_SPEED,
110            max_angular_speed: DEFAULT_MAX_ANGULAR_SPEED,
111            goal_tolerance: DEFAULT_GOAL_TOLERANCE,
112        }
113    }
114}
115
116/// Shared input to every arena controller.
117#[derive(Debug, Clone)]
118pub struct ArenaScenario {
119    pub preset: ArenaPreset,
120    pub path: Path2D,
121    pub initial_state: State2D,
122    pub dt: f64,
123    pub target_speed: f64,
124    /// Multiplier applied to the requested angular velocity; 1.0 is ideal.
125    pub turn_rate_response_gain: f64,
126    pub max_steps: usize,
127    pub max_linear_speed: f64,
128    pub max_angular_speed: f64,
129    pub goal_tolerance: f64,
130}
131
132impl ArenaScenario {
133    pub(crate) fn validate(&self) -> RoboticsResult<()> {
134        if self.path.len() < 2 {
135            return Err(invalid("arena path must contain at least two points"));
136        }
137        if !self
138            .path
139            .points
140            .iter()
141            .all(|point| point.x.is_finite() && point.y.is_finite())
142        {
143            return Err(invalid("arena path coordinates must be finite"));
144        }
145        for (name, value) in [
146            ("dt", self.dt),
147            ("target_speed", self.target_speed),
148            ("turn_rate_response_gain", self.turn_rate_response_gain),
149            ("max_linear_speed", self.max_linear_speed),
150            ("max_angular_speed", self.max_angular_speed),
151            ("goal_tolerance", self.goal_tolerance),
152        ] {
153            if !value.is_finite() {
154                return Err(invalid(&format!("{name} must be finite")));
155            }
156        }
157        if self.dt <= 0.0
158            || self.target_speed <= 0.0
159            || !(0.0..=1.0).contains(&self.turn_rate_response_gain)
160            || self.max_steps == 0
161            || self.max_linear_speed <= 0.0
162            || self.max_angular_speed <= 0.0
163            || self.goal_tolerance <= 0.0
164        {
165            return Err(invalid(
166                "arena dynamics require positive limits and a response gain in 0..=1",
167            ));
168        }
169        if !state_is_finite(self.initial_state) {
170            return Err(invalid("arena initial state must be finite"));
171        }
172        Ok(())
173    }
174}
175
176/// One simulation sample. The initial sample uses a zero command at `time = 0`.
177#[derive(Debug, Clone, Copy, PartialEq)]
178pub struct ArenaSample {
179    pub time: f64,
180    pub state: State2D,
181    pub command: ControlInput,
182    pub cross_track_error: f64,
183}
184
185/// Metrics derived exclusively from an [`ArenaRun`]'s samples.
186#[derive(Debug, Clone, Copy, PartialEq)]
187pub struct ArenaMetrics {
188    pub cross_track_rmse: f64,
189    pub final_goal_distance: f64,
190    pub max_cross_track_error: f64,
191    pub angular_command_smoothness: f64,
192}
193
194/// Complete deterministic trace for one controller.
195#[derive(Debug, Clone, PartialEq)]
196pub struct ArenaRun {
197    pub controller: ArenaControllerKind,
198    pub samples: Vec<ArenaSample>,
199    pub metrics: ArenaMetrics,
200}
201
202/// Runs every requested controller under one shared scenario.
203pub fn run_controller_arena(
204    scenario: &ArenaScenario,
205    controllers: &[ArenaControllerKind],
206) -> RoboticsResult<Vec<ArenaRun>> {
207    scenario.validate()?;
208    if controllers.is_empty() {
209        return Err(invalid("at least one arena controller is required"));
210    }
211    controllers
212        .iter()
213        .copied()
214        .map(|kind| run_one(scenario, kind))
215        .collect()
216}
217
218/// Recomputes metrics from samples, providing one source of truth for tests and
219/// alternate front ends.
220pub fn metrics_from_samples(
221    samples: &[ArenaSample],
222    goal: Point2D,
223) -> RoboticsResult<ArenaMetrics> {
224    if samples.is_empty() {
225        return Err(invalid("arena metrics require at least one sample"));
226    }
227    if !samples.iter().all(sample_is_finite) {
228        return Err(RoboticsError::NumericalError(
229            "arena samples contain a non-finite value".to_string(),
230        ));
231    }
232
233    let squared_error_sum = samples
234        .iter()
235        .map(|sample| sample.cross_track_error * sample.cross_track_error)
236        .sum::<f64>();
237    let max_cross_track_error = samples
238        .iter()
239        .map(|sample| sample.cross_track_error.abs())
240        .fold(0.0_f64, f64::max);
241    let squared_command_delta_sum = samples
242        .windows(2)
243        .map(|pair| {
244            let delta = pair[1].command.omega - pair[0].command.omega;
245            delta * delta
246        })
247        .sum::<f64>();
248    let command_delta_count = samples.len().saturating_sub(1).max(1);
249    let final_state = samples
250        .last()
251        .expect("non-empty samples were checked")
252        .state;
253
254    Ok(ArenaMetrics {
255        cross_track_rmse: (squared_error_sum / samples.len() as f64).sqrt(),
256        final_goal_distance: final_state.position().distance(&goal),
257        max_cross_track_error,
258        angular_command_smoothness: (squared_command_delta_sum / command_delta_count as f64).sqrt(),
259    })
260}
261
262fn run_one(scenario: &ArenaScenario, kind: ArenaControllerKind) -> RoboticsResult<ArenaRun> {
263    let mut tracker = build_tracker(kind, scenario);
264    let goal = *scenario
265        .path
266        .points
267        .last()
268        .expect("validated path has at least two points");
269    // The run also ends when the vehicle crosses the line through the goal,
270    // perpendicular to the last segment, close to the goal: a controller that
271    // passes the end off to the side (or circles it) would otherwise drive on
272    // until max_steps. "Close" keeps a loop course, whose end line the
273    // vehicle may cross elsewhere on the way round, from ending early.
274    let points = &scenario.path.points;
275    let before_goal = points[points.len() - 2];
276    let end_direction = (goal.x - before_goal.x, goal.y - before_goal.y);
277    let past_end = |s: State2D| (s.x - goal.x) * end_direction.0 + (s.y - goal.y) * end_direction.1;
278    let mut state = scenario.initial_state;
279    let mut samples = Vec::with_capacity(scenario.max_steps + 1);
280    samples.push(ArenaSample {
281        time: 0.0,
282        state,
283        command: ControlInput::zero(),
284        cross_track_error: cross_track_error(&scenario.path, state),
285    });
286
287    for step in 1..=scenario.max_steps {
288        let requested = tracker.compute_control(&state, &scenario.path);
289        if !requested.v.is_finite() || !requested.omega.is_finite() {
290            return Err(RoboticsError::NumericalError(format!(
291                "{} returned a non-finite command",
292                kind.label()
293            )));
294        }
295
296        // Controllers contribute steering curvature. Longitudinal response is
297        // owned by the arena so all controllers receive the same speed model.
298        let command = apply_shared_response(scenario, state, requested);
299        state = propagate(state, command, scenario.dt);
300        let sample = ArenaSample {
301            time: step as f64 * scenario.dt,
302            state,
303            command,
304            cross_track_error: cross_track_error(&scenario.path, state),
305        };
306        if !sample_is_finite(&sample) {
307            return Err(RoboticsError::NumericalError(format!(
308                "{} produced a non-finite state",
309                kind.label()
310            )));
311        }
312        let previous = samples.last().expect("initial sample").state;
313        samples.push(sample);
314
315        let to_goal = state.position().distance(&goal);
316        if to_goal <= scenario.goal_tolerance {
317            break;
318        }
319        if past_end(previous) <= 0.0 && past_end(state) > 0.0 && to_goal < END_LINE_RADIUS {
320            break;
321        }
322    }
323
324    let metrics = metrics_from_samples(&samples, goal)?;
325    Ok(ArenaRun {
326        controller: kind,
327        samples,
328        metrics,
329    })
330}
331
332/// Shared longitudinal/turn-rate response applied identically to every
333/// controller (arena and meta-control runs). Controllers contribute steering
334/// curvature; the arena owns the speed model so all controllers see the same
335/// actuation.
336pub(crate) fn apply_shared_response(
337    scenario: &ArenaScenario,
338    state: State2D,
339    requested: ControlInput,
340) -> ControlInput {
341    let curvature = if requested.v.abs() > MIN_CURVATURE_SPEED {
342        requested.omega / requested.v
343    } else {
344        0.0
345    };
346    let speed_alpha = (SPEED_RESPONSE_PER_SECOND * scenario.dt).clamp(0.0, 1.0);
347    let v = (state.v + (scenario.target_speed - state.v) * speed_alpha)
348        .clamp(0.0, scenario.max_linear_speed);
349    let omega = (curvature * v * scenario.turn_rate_response_gain)
350        .clamp(-scenario.max_angular_speed, scenario.max_angular_speed);
351    ControlInput::new(v, omega)
352}
353
354pub(crate) fn build_tracker(
355    kind: ArenaControllerKind,
356    scenario: &ArenaScenario,
357) -> Box<dyn PathTracker> {
358    match kind {
359        ArenaControllerKind::PurePursuit => {
360            Box::new(PurePursuitController::new(PurePursuitConfig {
361                look_ahead_gain: 0.12,
362                look_ahead_distance: 2.4,
363                wheelbase: 2.9,
364                kp: 1.0,
365                goal_threshold: scenario.goal_tolerance,
366            }))
367        }
368        ArenaControllerKind::Stanley => Box::new(StanleyController::new(StanleyConfig {
369            k: 0.55,
370            wheelbase: 2.9,
371            kp: 1.0,
372            goal_threshold: scenario.goal_tolerance,
373        })),
374        ArenaControllerKind::LqrSteer => {
375            let mut tracker = LQRSteerController::new(LQRSteerConfig {
376                wheelbase: 2.9,
377                max_steer: 45.0_f64.to_radians(),
378                kp: 1.0,
379                q_diag: [1.0, 1.0, 1.0, 1.0],
380                r: 1.0,
381                dt: scenario.dt,
382                goal_threshold: scenario.goal_tolerance,
383            });
384            tracker.set_path_with_speed(scenario.path.clone(), scenario.target_speed);
385            Box::new(tracker)
386        }
387    }
388}
389
390pub(crate) fn propagate(state: State2D, command: ControlInput, dt: f64) -> State2D {
391    let x = state.x + command.v * state.yaw.cos() * dt;
392    let y = state.y + command.v * state.yaw.sin() * dt;
393    let yaw = normalize_angle(state.yaw + command.omega * dt);
394    State2D::new(x, y, yaw, command.v)
395}
396
397pub(crate) use rust_robotics_core::normalize_angle;
398
399pub(crate) fn cross_track_error(path: &Path2D, state: State2D) -> f64 {
400    let position = state.position();
401    path.nearest_point_index(position)
402        .map(|index| position.distance(&path.points[index]))
403        .unwrap_or(f64::INFINITY)
404}
405
406fn straight_path() -> Path2D {
407    Path2D::from_points(
408        (0..=100)
409            .map(|index| Point2D::new(index as f64 * 0.5, 0.0))
410            .collect(),
411    )
412}
413
414fn slalom_path() -> Path2D {
415    Path2D::from_points(
416        (0..=120)
417            .map(|index| {
418                let x = index as f64 * 0.5;
419                Point2D::new(x, 3.0 * (x / 7.0).sin())
420            })
421            .collect(),
422    )
423}
424
425fn hairpin_path() -> Path2D {
426    let mut points = Vec::new();
427    for index in 0..=40 {
428        points.push(Point2D::new(index as f64 * 0.5, 0.0));
429    }
430    let radius = 6.0;
431    for index in 1..=38 {
432        let theta = -std::f64::consts::FRAC_PI_2 + std::f64::consts::PI * index as f64 / 38.0;
433        points.push(Point2D::new(
434            20.0 + radius * theta.cos(),
435            6.0 + radius * theta.sin(),
436        ));
437    }
438    for index in 1..=40 {
439        points.push(Point2D::new(20.0 - index as f64 * 0.5, 12.0));
440    }
441    Path2D::from_points(points)
442}
443
444pub(crate) fn state_is_finite(state: State2D) -> bool {
445    state.x.is_finite() && state.y.is_finite() && state.yaw.is_finite() && state.v.is_finite()
446}
447
448pub(crate) fn sample_is_finite(sample: &ArenaSample) -> bool {
449    sample.time.is_finite()
450        && state_is_finite(sample.state)
451        && sample.command.v.is_finite()
452        && sample.command.omega.is_finite()
453        && sample.cross_track_error.is_finite()
454}
455
456pub(crate) fn invalid(message: &str) -> RoboticsError {
457    RoboticsError::InvalidParameter(message.to_string())
458}
459
460#[cfg(test)]
461mod tests {
462    use super::*;
463
464    fn straight(target_speed: f64) -> ArenaScenario {
465        ArenaPreset::StraightRecovery.scenario(target_speed, 0.85)
466    }
467
468    #[test]
469    fn pure_pursuit_finishes_when_the_goal_is_inside_its_look_ahead() {
470        // Starting near the end: the last point is closer than the look-ahead
471        // distance, and Pure Pursuit used to circle it until max_steps.
472        let mut scenario = straight(3.0);
473        scenario.initial_state = State2D::new(45.0, 2.5, 0.0, 0.5);
474        let run = run_controller_arena(&scenario, &[ArenaControllerKind::PurePursuit])
475            .unwrap()
476            .remove(0);
477        assert!(
478            run.samples.len() < scenario.max_steps / 2,
479            "{} steps",
480            run.samples.len()
481        );
482        assert!(run.metrics.final_goal_distance < 1.5);
483    }
484
485    #[test]
486    fn trackers_follow_a_loop_whose_end_is_next_to_its_start() {
487        // 340° of a circle: the end passes 1.4 m from where the car starts.
488        let radius = 10.0;
489        let path = Path2D::from_points(
490            (0..=340)
491                .map(|deg| {
492                    let a = (deg as f64).to_radians() - std::f64::consts::FRAC_PI_2;
493                    Point2D::new(radius * a.cos(), radius + radius * a.sin())
494                })
495                .collect(),
496        );
497        let length: f64 = path.points.windows(2).map(|w| w[0].distance(&w[1])).sum();
498        let mut scenario = straight(3.0);
499        scenario.initial_state = State2D::new(-1.0, 0.0, 0.0, 0.5);
500        scenario.max_steps = 600;
501        scenario.path = path;
502        for run in run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap() {
503            let duration = run.samples.last().unwrap().time;
504            assert!(
505                duration > 0.8 * length / 3.0,
506                "{} skipped the loop: done after {duration:.1} s",
507                run.controller.label()
508            );
509            assert!(
510                run.metrics.final_goal_distance < 1.5,
511                "{}",
512                run.controller.label()
513            );
514            assert!(
515                run.metrics.max_cross_track_error < 2.0,
516                "{}",
517                run.controller.label()
518            );
519        }
520    }
521
522    #[test]
523    fn a_run_that_overshoots_the_end_stops_there() {
524        // Starting close to the end and off to the side, Stanley and LQR pass
525        // the goal more than the tolerance away; they used to drive on for
526        // the whole step budget.
527        let mut scenario = straight(3.0);
528        scenario.initial_state = State2D::new(47.0, 1.8, 0.0, 0.5);
529        for run in run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap() {
530            assert!(
531                run.metrics.final_goal_distance < 5.0,
532                "{} ended {:.1} m away",
533                run.controller.label(),
534                run.metrics.final_goal_distance
535            );
536        }
537    }
538
539    const TOLERANCE: f64 = 1e-12;
540
541    #[test]
542    fn presets_are_finite_and_continuous() {
543        for preset in ArenaPreset::ALL {
544            let scenario = preset.scenario(3.0, 0.85);
545            scenario.validate().unwrap();
546            assert!(scenario.path.len() >= 100);
547            assert!(scenario.path.total_length() > 45.0);
548            assert!(scenario.path.points.windows(2).all(|pair| {
549                let spacing = pair[0].distance(&pair[1]);
550                spacing > 0.0 && spacing < 0.7
551            }));
552        }
553    }
554
555    #[test]
556    fn every_controller_runs_every_preset_with_shared_initial_state() {
557        for preset in ArenaPreset::ALL {
558            let scenario = preset.scenario(3.0, 0.85);
559            let runs = run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap();
560            assert_eq!(runs.len(), ArenaControllerKind::ALL.len());
561            for run in runs {
562                assert!(
563                    run.samples.len() > 1,
564                    "{} has no trace",
565                    run.controller.label()
566                );
567                assert_eq!(run.samples[0].state, scenario.initial_state);
568                assert!(run.samples.iter().all(sample_is_finite));
569                assert!(run.samples.iter().all(|sample| {
570                    sample.command.v.abs() <= scenario.max_linear_speed + TOLERANCE
571                        && sample.command.omega.abs() <= scenario.max_angular_speed + TOLERANCE
572                }));
573                assert!(metrics_are_finite(run.metrics));
574            }
575        }
576    }
577
578    #[test]
579    fn repeated_runs_are_deterministic() {
580        for preset in ArenaPreset::ALL {
581            let scenario = preset.scenario(2.75, 0.72);
582            let first = run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap();
583            let second = run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap();
584            assert_eq!(first, second);
585        }
586    }
587
588    #[test]
589    fn stored_metrics_match_recomputation() {
590        for preset in ArenaPreset::ALL {
591            let scenario = preset.scenario(3.5, 1.0);
592            let goal = *scenario.path.points.last().unwrap();
593            for run in run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap() {
594                let recomputed = metrics_from_samples(&run.samples, goal).unwrap();
595                assert_metrics_close(run.metrics, recomputed);
596            }
597        }
598    }
599
600    #[test]
601    fn invalid_scenarios_and_empty_controller_lists_are_rejected() {
602        let mut scenario = ArenaPreset::StraightRecovery.scenario(3.0, 1.0);
603        assert!(run_controller_arena(&scenario, &[]).is_err());
604        scenario.turn_rate_response_gain = 1.1;
605        assert!(run_controller_arena(&scenario, &ArenaControllerKind::ALL).is_err());
606        scenario = ArenaPreset::StraightRecovery.scenario(3.0, 1.0);
607        scenario.path = Path2D::new();
608        assert!(run_controller_arena(&scenario, &ArenaControllerKind::ALL).is_err());
609    }
610
611    fn metrics_are_finite(metrics: ArenaMetrics) -> bool {
612        metrics.cross_track_rmse.is_finite()
613            && metrics.final_goal_distance.is_finite()
614            && metrics.max_cross_track_error.is_finite()
615            && metrics.angular_command_smoothness.is_finite()
616    }
617
618    fn assert_metrics_close(left: ArenaMetrics, right: ArenaMetrics) {
619        assert!((left.cross_track_rmse - right.cross_track_rmse).abs() <= TOLERANCE);
620        assert!((left.final_goal_distance - right.final_goal_distance).abs() <= TOLERANCE);
621        assert!((left.max_cross_track_error - right.max_cross_track_error).abs() <= TOLERANCE);
622        assert!(
623            (left.angular_command_smoothness - right.angular_command_smoothness).abs() <= TOLERANCE
624        );
625    }
626}