Skip to main content

rust_robotics_control/
mppi.rs

1//! Vanilla MPPI controller for a 2-D double integrator.
2//!
3//! This is the dependency-light baseline needed before TD-CD-MPPI-style
4//! terminal value and constraint-discounted extensions.
5
6use rand::{rngs::StdRng, SeedableRng};
7use rand_distr::{Distribution, Normal};
8use rust_robotics_core::{RoboticsError, RoboticsResult};
9use std::collections::VecDeque;
10
11/// State for a planar double-integrator model.
12#[derive(Debug, Clone, Copy, PartialEq)]
13pub struct MppiState2D {
14    pub x: f64,
15    pub y: f64,
16    pub vx: f64,
17    pub vy: f64,
18}
19
20impl MppiState2D {
21    pub fn new(x: f64, y: f64, vx: f64, vy: f64) -> Self {
22        Self { x, y, vx, vy }
23    }
24
25    pub fn step(self, control: MppiControl2D, dt: f64) -> Self {
26        Self {
27            x: self.x + self.vx * dt + 0.5 * control.ax * dt * dt,
28            y: self.y + self.vy * dt + 0.5 * control.ay * dt * dt,
29            vx: self.vx + control.ax * dt,
30            vy: self.vy + control.ay * dt,
31        }
32    }
33}
34
35/// Acceleration command for the double-integrator model.
36#[derive(Debug, Clone, Copy, PartialEq)]
37pub struct MppiControl2D {
38    pub ax: f64,
39    pub ay: f64,
40}
41
42impl MppiControl2D {
43    pub fn new(ax: f64, ay: f64) -> Self {
44        Self { ax, ay }
45    }
46}
47
48/// Circular obstacle used by constraint-discounted MPPI rollouts.
49#[derive(Debug, Clone, Copy, PartialEq)]
50pub struct MppiCircularObstacle2D {
51    pub x: f64,
52    pub y: f64,
53    pub radius: f64,
54}
55
56impl MppiCircularObstacle2D {
57    pub fn new(x: f64, y: f64, radius: f64) -> Self {
58        Self { x, y, radius }
59    }
60}
61
62/// Linearly predicted circular obstacle for prediction-aware MPPI rollouts.
63#[derive(Debug, Clone, Copy, PartialEq)]
64pub struct MppiMovingObstacle2D {
65    pub x: f64,
66    pub y: f64,
67    pub vx: f64,
68    pub vy: f64,
69    pub radius: f64,
70}
71
72impl MppiMovingObstacle2D {
73    pub fn new(x: f64, y: f64, vx: f64, vy: f64, radius: f64) -> Self {
74        Self {
75            x,
76            y,
77            vx,
78            vy,
79            radius,
80        }
81    }
82
83    pub fn predict(self, time: f64) -> MppiCircularObstacle2D {
84        MppiCircularObstacle2D::new(
85            self.x + self.vx * time,
86            self.y + self.vy * time,
87            self.radius,
88        )
89    }
90}
91
92/// 2-D racing gate for reference-free MPPI progress objectives.
93///
94/// The normal points in the race direction. A gate is crossed when a rollout
95/// segment moves from the negative to the positive side of the gate plane and
96/// intersects the gate within `half_width`.
97#[derive(Debug, Clone, Copy, PartialEq)]
98pub struct MppiRacingGate2D {
99    pub center_x: f64,
100    pub center_y: f64,
101    pub normal_x: f64,
102    pub normal_y: f64,
103    pub half_width: f64,
104}
105
106impl MppiRacingGate2D {
107    pub fn new(
108        center_x: f64,
109        center_y: f64,
110        normal_x: f64,
111        normal_y: f64,
112        half_width: f64,
113    ) -> RoboticsResult<Self> {
114        let norm = (normal_x * normal_x + normal_y * normal_y).sqrt();
115        if !center_x.is_finite()
116            || !center_y.is_finite()
117            || !normal_x.is_finite()
118            || !normal_y.is_finite()
119            || norm <= 0.0
120        {
121            return Err(RoboticsError::InvalidParameter(
122                "racing gate center and normal must be finite".to_string(),
123            ));
124        }
125        if half_width <= 0.0 || !half_width.is_finite() {
126            return Err(RoboticsError::InvalidParameter(
127                "racing gate half_width must be finite and positive".to_string(),
128            ));
129        }
130        Ok(Self {
131            center_x,
132            center_y,
133            normal_x: normal_x / norm,
134            normal_y: normal_y / norm,
135            half_width,
136        })
137    }
138
139    pub fn center(&self) -> (f64, f64) {
140        (self.center_x, self.center_y)
141    }
142
143    pub fn signed_distance(&self, x: f64, y: f64) -> f64 {
144        (x - self.center_x) * self.normal_x + (y - self.center_y) * self.normal_y
145    }
146
147    pub fn lateral_distance(&self, x: f64, y: f64) -> f64 {
148        let tangent_x = -self.normal_y;
149        let tangent_y = self.normal_x;
150        ((x - self.center_x) * tangent_x + (y - self.center_y) * tangent_y).abs()
151    }
152
153    pub fn squared_distance(&self, x: f64, y: f64) -> f64 {
154        (x - self.center_x).powi(2) + (y - self.center_y).powi(2)
155    }
156
157    pub fn crosses(&self, from: MppiState2D, to: MppiState2D, tolerance: f64) -> bool {
158        let from_signed = self.signed_distance(from.x, from.y);
159        let to_signed = self.signed_distance(to.x, to.y);
160        if from_signed > tolerance || to_signed < -tolerance {
161            return false;
162        }
163        let denom = to_signed - from_signed;
164        let t = if denom.abs() > 1e-9 {
165            (-from_signed / denom).clamp(0.0, 1.0)
166        } else {
167            0.0
168        };
169        let cross_x = from.x + t * (to.x - from.x);
170        let cross_y = from.y + t * (to.y - from.y);
171        self.lateral_distance(cross_x, cross_y) <= self.half_width + tolerance
172    }
173
174    pub fn state_is_past_gate(&self, state: MppiState2D, tolerance: f64) -> bool {
175        self.signed_distance(state.x, state.y) >= -tolerance
176            && self.lateral_distance(state.x, state.y) <= self.half_width + tolerance
177    }
178}
179
180/// Reference-free gate-progress objective for race-style MPPI.
181///
182/// The per-step progress term follows the racing-MPPI idea of minimizing the
183/// change in squared distance to the active gate. Moving toward the gate creates
184/// negative cost, moving away creates positive cost, and a valid gate crossing
185/// switches the rollout to the next gate.
186#[derive(Debug, Clone, PartialEq)]
187pub struct MppiGateRace2D {
188    pub gates: Vec<MppiRacingGate2D>,
189    pub progress_weight: f64,
190    pub lateral_weight: f64,
191    pub pass_bonus: f64,
192    pub miss_penalty: f64,
193    pub terminal_gate_weight: f64,
194    pub crossing_tolerance: f64,
195    active_gate_index: usize,
196}
197
198/// Summary for scoring a trajectory against a gate race.
199#[derive(Debug, Clone, Copy, PartialEq)]
200pub struct MppiGateRaceReport2D {
201    pub passed_gates: usize,
202    pub active_gate_index: usize,
203    pub cost: f64,
204    pub final_gate_distance: f64,
205    pub final_lateral_error: f64,
206}
207
208#[derive(Debug, Clone, Copy, PartialEq)]
209struct MppiGateRaceStep2D {
210    cost: f64,
211    active_gate_index: usize,
212    passed_gate: bool,
213}
214
215impl MppiGateRace2D {
216    pub fn new(gates: Vec<MppiRacingGate2D>) -> RoboticsResult<Self> {
217        let race = Self {
218            gates,
219            progress_weight: 1.0,
220            lateral_weight: 0.08,
221            pass_bonus: 4.0,
222            miss_penalty: 18.0,
223            terminal_gate_weight: 0.6,
224            crossing_tolerance: 0.03,
225            active_gate_index: 0,
226        };
227        validate_gate_race(&race)?;
228        Ok(race)
229    }
230
231    pub fn gate_count(&self) -> usize {
232        self.gates.len()
233    }
234
235    pub fn active_gate_index(&self) -> usize {
236        self.active_gate_index
237    }
238
239    pub fn reset(&mut self) {
240        self.active_gate_index = 0;
241    }
242
243    pub fn set_active_gate_index(&mut self, active_gate_index: usize) -> RoboticsResult<()> {
244        if active_gate_index > self.gates.len() {
245            return Err(RoboticsError::InvalidParameter(
246                "active gate index must be within the gate list".to_string(),
247            ));
248        }
249        self.active_gate_index = active_gate_index;
250        Ok(())
251    }
252
253    pub fn advance_active_gate_from_state(&mut self, state: MppiState2D) -> usize {
254        while let Some(gate) = self.gates.get(self.active_gate_index) {
255            if !gate.state_is_past_gate(state, self.crossing_tolerance) {
256                break;
257            }
258            self.active_gate_index += 1;
259        }
260        self.active_gate_index
261    }
262
263    pub fn score_trajectory(&self, states: &[MppiState2D]) -> RoboticsResult<MppiGateRaceReport2D> {
264        if states.is_empty() {
265            return Err(RoboticsError::InvalidParameter(
266                "gate race trajectory must contain at least one state".to_string(),
267            ));
268        }
269        for &state in states {
270            validate_state(state)?;
271        }
272        let mut active_gate_index = 0;
273        let mut cost = 0.0;
274        let mut passed_gates = 0;
275        for pair in states.windows(2) {
276            let step = self.transition_cost(pair[0], pair[1], active_gate_index);
277            cost += step.cost;
278            active_gate_index = step.active_gate_index;
279            if step.passed_gate {
280                passed_gates += 1;
281            }
282        }
283        let final_state = *states
284            .last()
285            .expect("validated gate race trajectory is non-empty");
286        cost += self.terminal_cost(final_state, active_gate_index);
287        let (final_gate_distance, final_lateral_error) =
288            self.final_gate_errors(final_state, active_gate_index);
289        Ok(MppiGateRaceReport2D {
290            passed_gates,
291            active_gate_index,
292            cost,
293            final_gate_distance,
294            final_lateral_error,
295        })
296    }
297
298    fn transition_cost(
299        &self,
300        from: MppiState2D,
301        to: MppiState2D,
302        active_gate_index: usize,
303    ) -> MppiGateRaceStep2D {
304        let Some(gate) = self.gates.get(active_gate_index) else {
305            return MppiGateRaceStep2D {
306                cost: 0.0,
307                active_gate_index,
308                passed_gate: false,
309            };
310        };
311
312        let before_distance_sq = gate.squared_distance(from.x, from.y);
313        let after_distance_sq = gate.squared_distance(to.x, to.y);
314        let after_lateral = gate.lateral_distance(to.x, to.y);
315        let after_distance = after_distance_sq.sqrt();
316        let lateral_proximity = 1.0 / (1.0 + after_distance);
317        let mut cost = self.progress_weight * (after_distance_sq - before_distance_sq)
318            + self.lateral_weight * lateral_proximity * after_lateral * after_lateral;
319        let mut next_active = active_gate_index;
320        let mut passed_gate = false;
321
322        if gate.crosses(from, to, self.crossing_tolerance) {
323            cost -= self.pass_bonus;
324            next_active += 1;
325            passed_gate = true;
326        } else if gate.signed_distance(to.x, to.y) >= 0.0 && after_lateral > gate.half_width {
327            let miss = after_lateral - gate.half_width;
328            cost += self.miss_penalty * miss * miss;
329        }
330
331        MppiGateRaceStep2D {
332            cost,
333            active_gate_index: next_active,
334            passed_gate,
335        }
336    }
337
338    fn terminal_cost(&self, state: MppiState2D, active_gate_index: usize) -> f64 {
339        let Some(gate) = self.gates.get(active_gate_index) else {
340            return 0.0;
341        };
342        self.terminal_gate_weight * gate.squared_distance(state.x, state.y)
343            + self.pass_bonus * (self.gates.len() - active_gate_index) as f64
344    }
345
346    fn final_gate_errors(&self, state: MppiState2D, active_gate_index: usize) -> (f64, f64) {
347        let Some(gate) = self.gates.get(active_gate_index) else {
348            return (0.0, 0.0);
349        };
350        (
351            gate.squared_distance(state.x, state.y).sqrt(),
352            gate.lateral_distance(state.x, state.y),
353        )
354    }
355}
356
357/// Grid-based terminal value estimate for MPPI rollouts.
358///
359/// Values are indexed as `values[x][y]` and queried with bilinear
360/// interpolation. Out-of-bounds queries clamp to the nearest grid edge.
361#[derive(Debug, Clone, PartialEq)]
362pub struct MppiTerminalValueGrid2D {
363    pub origin_x: f64,
364    pub origin_y: f64,
365    pub resolution: f64,
366    pub values: Vec<Vec<f64>>,
367}
368
369impl MppiTerminalValueGrid2D {
370    pub fn new(
371        origin_x: f64,
372        origin_y: f64,
373        resolution: f64,
374        values: Vec<Vec<f64>>,
375    ) -> RoboticsResult<Self> {
376        let grid = Self {
377            origin_x,
378            origin_y,
379            resolution,
380            values,
381        };
382        validate_terminal_value_grid(&grid)?;
383        Ok(grid)
384    }
385
386    pub fn from_goal_distance(
387        width: usize,
388        height: usize,
389        origin_x: f64,
390        origin_y: f64,
391        resolution: f64,
392        goal: (f64, f64),
393    ) -> RoboticsResult<Self> {
394        if width == 0 || height == 0 {
395            return Err(RoboticsError::InvalidParameter(
396                "terminal value grid dimensions must be non-empty".to_string(),
397            ));
398        }
399        let mut values = vec![vec![0.0; height]; width];
400        for (x, column) in values.iter_mut().enumerate() {
401            for (y, value) in column.iter_mut().enumerate() {
402                let wx = origin_x + x as f64 * resolution;
403                let wy = origin_y + y as f64 * resolution;
404                let dx = wx - goal.0;
405                let dy = wy - goal.1;
406                *value = (dx * dx + dy * dy).sqrt();
407            }
408        }
409        Self::new(origin_x, origin_y, resolution, values)
410    }
411
412    pub fn value_at_state(&self, state: MppiState2D) -> f64 {
413        self.value_at(state.x, state.y)
414    }
415
416    pub fn value_at(&self, x: f64, y: f64) -> f64 {
417        let width = self.values.len();
418        let height = self.values[0].len();
419        let gx = ((x - self.origin_x) / self.resolution).clamp(0.0, (width - 1) as f64);
420        let gy = ((y - self.origin_y) / self.resolution).clamp(0.0, (height - 1) as f64);
421        let x0 = gx.floor() as usize;
422        let y0 = gy.floor() as usize;
423        let x1 = (x0 + 1).min(width - 1);
424        let y1 = (y0 + 1).min(height - 1);
425        let tx = gx - x0 as f64;
426        let ty = gy - y0 as f64;
427
428        let v00 = self.values[x0][y0];
429        let v10 = self.values[x1][y0];
430        let v01 = self.values[x0][y1];
431        let v11 = self.values[x1][y1];
432        let vx0 = v00 * (1.0 - tx) + v10 * tx;
433        let vx1 = v01 * (1.0 - tx) + v11 * tx;
434        vx0 * (1.0 - ty) + vx1 * ty
435    }
436
437    pub fn width(&self) -> usize {
438        self.values.len()
439    }
440
441    pub fn height(&self) -> usize {
442        self.values[0].len()
443    }
444
445    pub fn nearest_cell_indices(&self, x: f64, y: f64) -> (usize, usize) {
446        let gx = ((x - self.origin_x) / self.resolution)
447            .round()
448            .clamp(0.0, (self.width() - 1) as f64);
449        let gy = ((y - self.origin_y) / self.resolution)
450            .round()
451            .clamp(0.0, (self.height() - 1) as f64);
452        (gx as usize, gy as usize)
453    }
454
455    pub fn value_at_cell(&self, x: usize, y: usize) -> RoboticsResult<f64> {
456        if x >= self.width() || y >= self.height() {
457            return Err(RoboticsError::InvalidParameter(
458                "terminal value grid cell is out of bounds".to_string(),
459            ));
460        }
461        Ok(self.values[x][y])
462    }
463
464    pub fn update_cell_toward(
465        &mut self,
466        x: usize,
467        y: usize,
468        target: f64,
469        learning_rate: f64,
470    ) -> RoboticsResult<f64> {
471        if x >= self.width() || y >= self.height() {
472            return Err(RoboticsError::InvalidParameter(
473                "terminal value grid cell is out of bounds".to_string(),
474            ));
475        }
476        if target < 0.0 || !target.is_finite() {
477            return Err(RoboticsError::InvalidParameter(
478                "terminal value target must be finite and non-negative".to_string(),
479            ));
480        }
481        if !(0.0..=1.0).contains(&learning_rate) || learning_rate == 0.0 {
482            return Err(RoboticsError::InvalidParameter(
483                "terminal value learning_rate must be in (0, 1]".to_string(),
484            ));
485        }
486        let old = self.values[x][y];
487        let new_value = old + learning_rate * (target - old);
488        self.values[x][y] = new_value.max(0.0);
489        Ok((self.values[x][y] - old).abs())
490    }
491}
492
493/// Projection of a point onto a waypoint track.
494#[derive(Debug, Clone, Copy, PartialEq)]
495pub struct MppiTrackProjection2D {
496    pub segment_index: usize,
497    pub progress: f64,
498    pub lateral_error: f64,
499    pub closest_x: f64,
500    pub closest_y: f64,
501}
502
503/// Polyline track helper for MPPI progress-based terminal values.
504#[derive(Debug, Clone, PartialEq)]
505pub struct MppiWaypointTrack2D {
506    pub waypoints: Vec<(f64, f64)>,
507    cumulative_lengths: Vec<f64>,
508}
509
510impl MppiWaypointTrack2D {
511    pub fn new(waypoints: Vec<(f64, f64)>) -> RoboticsResult<Self> {
512        validate_track_waypoints(&waypoints)?;
513        let mut cumulative_lengths = vec![0.0; waypoints.len()];
514        for index in 1..waypoints.len() {
515            cumulative_lengths[index] = cumulative_lengths[index - 1]
516                + point_distance(waypoints[index - 1], waypoints[index]);
517        }
518        Ok(Self {
519            waypoints,
520            cumulative_lengths,
521        })
522    }
523
524    pub fn total_length(&self) -> f64 {
525        *self
526            .cumulative_lengths
527            .last()
528            .expect("validated track has at least two waypoints")
529    }
530
531    pub fn goal(&self) -> (f64, f64) {
532        *self
533            .waypoints
534            .last()
535            .expect("validated track has at least two waypoints")
536    }
537
538    pub fn point_at_progress(&self, progress: f64) -> RoboticsResult<(f64, f64)> {
539        if !progress.is_finite() {
540            return Err(RoboticsError::InvalidParameter(
541                "track progress query must be finite".to_string(),
542            ));
543        }
544        let clamped = progress.clamp(0.0, self.total_length());
545        for index in 0..self.waypoints.len() - 1 {
546            let segment_start = self.cumulative_lengths[index];
547            let segment_end = self.cumulative_lengths[index + 1];
548            if clamped <= segment_end || index + 2 == self.waypoints.len() {
549                let a = self.waypoints[index];
550                let b = self.waypoints[index + 1];
551                let segment_length = segment_end - segment_start;
552                let t = if segment_length > 0.0 {
553                    (clamped - segment_start) / segment_length
554                } else {
555                    0.0
556                };
557                return Ok((a.0 + t * (b.0 - a.0), a.1 + t * (b.1 - a.1)));
558            }
559        }
560        Ok(self.goal())
561    }
562
563    pub fn project(&self, x: f64, y: f64) -> RoboticsResult<MppiTrackProjection2D> {
564        if !x.is_finite() || !y.is_finite() {
565            return Err(RoboticsError::InvalidParameter(
566                "track projection point must be finite".to_string(),
567            ));
568        }
569
570        let mut best = MppiTrackProjection2D {
571            segment_index: 0,
572            progress: 0.0,
573            lateral_error: f64::INFINITY,
574            closest_x: self.waypoints[0].0,
575            closest_y: self.waypoints[0].1,
576        };
577        for index in 0..self.waypoints.len() - 1 {
578            let a = self.waypoints[index];
579            let b = self.waypoints[index + 1];
580            let dx = b.0 - a.0;
581            let dy = b.1 - a.1;
582            let segment_length_sq = dx * dx + dy * dy;
583            let t = (((x - a.0) * dx + (y - a.1) * dy) / segment_length_sq).clamp(0.0, 1.0);
584            let closest_x = a.0 + t * dx;
585            let closest_y = a.1 + t * dy;
586            let lateral_error = ((x - closest_x).powi(2) + (y - closest_y).powi(2)).sqrt();
587            if lateral_error < best.lateral_error {
588                best = MppiTrackProjection2D {
589                    segment_index: index,
590                    progress: self.cumulative_lengths[index] + t * segment_length_sq.sqrt(),
591                    lateral_error,
592                    closest_x,
593                    closest_y,
594                };
595            }
596        }
597        Ok(best)
598    }
599
600    pub fn remaining_distance(&self, x: f64, y: f64) -> RoboticsResult<f64> {
601        Ok((self.total_length() - self.project(x, y)?.progress).max(0.0))
602    }
603
604    #[allow(clippy::too_many_arguments)]
605    pub fn terminal_value_grid(
606        &self,
607        width: usize,
608        height: usize,
609        origin_x: f64,
610        origin_y: f64,
611        resolution: f64,
612        progress_weight: f64,
613        lateral_weight: f64,
614    ) -> RoboticsResult<MppiTerminalValueGrid2D> {
615        if width == 0 || height == 0 {
616            return Err(RoboticsError::InvalidParameter(
617                "track terminal value grid dimensions must be non-empty".to_string(),
618            ));
619        }
620        if progress_weight < 0.0 || lateral_weight < 0.0 {
621            return Err(RoboticsError::InvalidParameter(
622                "track terminal value weights must be non-negative".to_string(),
623            ));
624        }
625        if !progress_weight.is_finite() || !lateral_weight.is_finite() {
626            return Err(RoboticsError::InvalidParameter(
627                "track terminal value weights must be finite".to_string(),
628            ));
629        }
630
631        let mut values = vec![vec![0.0; height]; width];
632        for (grid_x, column) in values.iter_mut().enumerate() {
633            for (grid_y, value) in column.iter_mut().enumerate() {
634                let wx = origin_x + grid_x as f64 * resolution;
635                let wy = origin_y + grid_y as f64 * resolution;
636                let projection = self.project(wx, wy)?;
637                let remaining = (self.total_length() - projection.progress).max(0.0);
638                *value = progress_weight * remaining + lateral_weight * projection.lateral_error;
639            }
640        }
641        MppiTerminalValueGrid2D::new(origin_x, origin_y, resolution, values)
642    }
643}
644
645/// Configuration for online terminal-value updates from MPPI rollouts.
646#[derive(Debug, Clone, Copy, PartialEq)]
647pub struct MppiTerminalValueUpdateConfig2D {
648    pub learning_rate: f64,
649    pub discount: f64,
650}
651
652impl Default for MppiTerminalValueUpdateConfig2D {
653    fn default() -> Self {
654        Self {
655            learning_rate: 0.25,
656            discount: 0.98,
657        }
658    }
659}
660
661/// Summary returned after updating a terminal value grid from one rollout.
662#[derive(Debug, Clone, Copy, PartialEq)]
663pub struct MppiTerminalValueUpdateReport2D {
664    pub updates: usize,
665    pub mean_abs_delta: f64,
666    pub max_abs_delta: f64,
667    pub start_target: f64,
668    pub terminal_target: f64,
669}
670
671/// Online updater for terminal value grids.
672pub struct MppiTerminalValueUpdater2D {
673    config: MppiTerminalValueUpdateConfig2D,
674}
675
676impl MppiTerminalValueUpdater2D {
677    pub fn new(config: MppiTerminalValueUpdateConfig2D) -> RoboticsResult<Self> {
678        validate_terminal_value_update_config(&config)?;
679        Ok(Self { config })
680    }
681
682    pub fn update_from_rollout(
683        &self,
684        grid: &mut MppiTerminalValueGrid2D,
685        rollout: &MppiRollout2D,
686    ) -> RoboticsResult<MppiTerminalValueUpdateReport2D> {
687        validate_rollout_for_terminal_value_update(rollout)?;
688        let targets = discounted_cost_to_go(&rollout.stage_costs, self.config.discount);
689        let mut updates = 0;
690        let mut total_abs_delta = 0.0;
691        let mut max_abs_delta: f64 = 0.0;
692
693        for (&state, &target) in rollout.states.iter().zip(&targets) {
694            let (x, y) = grid.nearest_cell_indices(state.x, state.y);
695            let delta = grid.update_cell_toward(x, y, target, self.config.learning_rate)?;
696            updates += 1;
697            total_abs_delta += delta;
698            max_abs_delta = max_abs_delta.max(delta);
699        }
700
701        Ok(MppiTerminalValueUpdateReport2D {
702            updates,
703            mean_abs_delta: total_abs_delta / updates as f64,
704            max_abs_delta,
705            start_target: targets[0],
706            terminal_target: *targets
707                .last()
708                .expect("validated rollout has at least one stage cost"),
709        })
710    }
711}
712
713/// Replay buffer for reusing recent MPPI rollouts in terminal-value learning.
714#[derive(Debug, Clone, PartialEq)]
715pub struct MppiTerminalValueReplayBuffer2D {
716    capacity: usize,
717    rollouts: VecDeque<MppiRollout2D>,
718}
719
720/// Summary returned after replay-buffer terminal-value updates.
721#[derive(Debug, Clone, Copy, PartialEq)]
722pub struct MppiTerminalValueReplayUpdateReport2D {
723    pub rollouts: usize,
724    pub updates: usize,
725    pub mean_abs_delta: f64,
726    pub max_abs_delta: f64,
727}
728
729impl MppiTerminalValueReplayBuffer2D {
730    pub fn new(capacity: usize) -> RoboticsResult<Self> {
731        if capacity == 0 {
732            return Err(RoboticsError::InvalidParameter(
733                "terminal value replay capacity must be positive".to_string(),
734            ));
735        }
736        Ok(Self {
737            capacity,
738            rollouts: VecDeque::with_capacity(capacity),
739        })
740    }
741
742    pub fn capacity(&self) -> usize {
743        self.capacity
744    }
745
746    pub fn len(&self) -> usize {
747        self.rollouts.len()
748    }
749
750    pub fn is_empty(&self) -> bool {
751        self.rollouts.is_empty()
752    }
753
754    pub fn push(&mut self, rollout: MppiRollout2D) -> RoboticsResult<()> {
755        validate_rollout_for_terminal_value_update(&rollout)?;
756        if self.rollouts.len() == self.capacity {
757            self.rollouts.pop_front();
758        }
759        self.rollouts.push_back(rollout);
760        Ok(())
761    }
762
763    pub fn update_grid(
764        &self,
765        grid: &mut MppiTerminalValueGrid2D,
766        updater: &MppiTerminalValueUpdater2D,
767    ) -> RoboticsResult<MppiTerminalValueReplayUpdateReport2D> {
768        if self.rollouts.is_empty() {
769            return Err(RoboticsError::InvalidParameter(
770                "terminal value replay buffer is empty".to_string(),
771            ));
772        }
773
774        let mut updates = 0;
775        let mut weighted_delta_sum = 0.0;
776        let mut max_abs_delta: f64 = 0.0;
777        for rollout in &self.rollouts {
778            let report = updater.update_from_rollout(grid, rollout)?;
779            updates += report.updates;
780            weighted_delta_sum += report.mean_abs_delta * report.updates as f64;
781            max_abs_delta = max_abs_delta.max(report.max_abs_delta);
782        }
783
784        Ok(MppiTerminalValueReplayUpdateReport2D {
785            rollouts: self.rollouts.len(),
786            updates,
787            mean_abs_delta: weighted_delta_sum / updates as f64,
788            max_abs_delta,
789        })
790    }
791}
792
793/// MPPI configuration.
794#[derive(Debug, Clone, PartialEq)]
795pub struct MppiConfig {
796    pub horizon: usize,
797    pub samples: usize,
798    pub dt: f64,
799    pub lambda: f64,
800    pub adaptive_lambda: bool,
801    pub min_lambda: f64,
802    pub max_lambda: f64,
803    pub target_effective_sample_ratio: f64,
804    pub adaptive_lambda_iterations: usize,
805    pub noise_sigma: f64,
806    pub control_limit: f64,
807    pub goal_weight: f64,
808    pub velocity_weight: f64,
809    pub control_weight: f64,
810    pub terminal_weight: f64,
811    pub constraint_weight: f64,
812    pub constraint_discount: f64,
813    pub safety_margin: f64,
814    pub obstacles: Vec<MppiCircularObstacle2D>,
815    pub moving_obstacles: Vec<MppiMovingObstacle2D>,
816    pub terminal_value_weight: f64,
817    pub terminal_value_grid: Option<MppiTerminalValueGrid2D>,
818    pub goal_trajectory: Option<Vec<(f64, f64)>>,
819    pub gate_race: Option<MppiGateRace2D>,
820    pub seed: u64,
821}
822
823impl Default for MppiConfig {
824    fn default() -> Self {
825        Self {
826            horizon: 20,
827            samples: 128,
828            dt: 0.1,
829            lambda: 1.0,
830            adaptive_lambda: false,
831            min_lambda: 0.05,
832            max_lambda: 10.0,
833            target_effective_sample_ratio: 0.25,
834            adaptive_lambda_iterations: 20,
835            noise_sigma: 0.8,
836            control_limit: 2.0,
837            goal_weight: 2.0,
838            velocity_weight: 0.1,
839            control_weight: 0.02,
840            terminal_weight: 8.0,
841            constraint_weight: 40.0,
842            constraint_discount: 0.95,
843            safety_margin: 0.05,
844            obstacles: Vec::new(),
845            moving_obstacles: Vec::new(),
846            terminal_value_weight: 1.0,
847            terminal_value_grid: None,
848            goal_trajectory: None,
849            gate_race: None,
850            seed: 7,
851        }
852    }
853}
854
855/// Diagnostics for MPPI path-integral sample weights.
856#[derive(Debug, Clone, Copy, PartialEq)]
857pub struct MppiSamplingDiagnostics2D {
858    pub sample_count: usize,
859    pub lambda: f64,
860    pub min_cost: f64,
861    pub mean_cost: f64,
862    pub max_cost: f64,
863    pub effective_sample_size: f64,
864    pub normalized_effective_sample_size: f64,
865    pub weight_entropy: f64,
866    pub normalized_weight_entropy: f64,
867}
868
869/// One MPPI rollout result.
870#[derive(Debug, Clone, PartialEq)]
871pub struct MppiRollout2D {
872    pub cost: f64,
873    pub model_cost: f64,
874    pub constraint_cost: f64,
875    pub terminal_value_cost: f64,
876    pub max_constraint_violation: f64,
877    pub states: Vec<MppiState2D>,
878    pub controls: Vec<MppiControl2D>,
879    pub stage_costs: Vec<f64>,
880}
881
882/// Result for one MPPI optimization step.
883#[derive(Debug, Clone, PartialEq)]
884pub struct MppiPlan2D {
885    pub first_control: MppiControl2D,
886    pub nominal_controls: Vec<MppiControl2D>,
887    pub best_rollout: MppiRollout2D,
888    pub sampling_diagnostics: MppiSamplingDiagnostics2D,
889}
890
891/// One MPPI step with its whole sample cloud
892/// ([`MppiController2D::plan_with_samples`]).
893#[derive(Debug, Clone, PartialEq)]
894pub struct MppiSampledPlan2D {
895    pub plan: MppiPlan2D,
896    /// Every rollout of the step; rollout 0 replays the previous nominal.
897    pub rollouts: Vec<MppiRollout2D>,
898    /// Path-integral weight of each rollout, normalized to sum to 1.
899    pub weights: Vec<f64>,
900}
901
902/// Vanilla MPPI controller.
903pub struct MppiController2D {
904    config: MppiConfig,
905    nominal_controls: Vec<MppiControl2D>,
906    rng: StdRng,
907}
908
909impl MppiController2D {
910    pub fn new(config: MppiConfig) -> RoboticsResult<Self> {
911        validate_config(&config)?;
912        Ok(Self {
913            nominal_controls: vec![MppiControl2D::new(0.0, 0.0); config.horizon],
914            rng: StdRng::seed_from_u64(config.seed),
915            config,
916        })
917    }
918
919    pub fn plan(&mut self, start: MppiState2D, goal: (f64, f64)) -> RoboticsResult<MppiPlan2D> {
920        self.plan_step(start, goal, false).map(|(plan, _)| plan)
921    }
922
923    /// [`plan`](Self::plan), also returning every sampled rollout with its
924    /// normalized path-integral weight (for visualizing the sample cloud).
925    /// Rollout 0 replays the previous nominal controls.
926    pub fn plan_with_samples(
927        &mut self,
928        start: MppiState2D,
929        goal: (f64, f64),
930    ) -> RoboticsResult<MppiSampledPlan2D> {
931        let (plan, samples) = self.plan_step(start, goal, true)?;
932        let (rollouts, weights) = samples.expect("samples were requested");
933        Ok(MppiSampledPlan2D {
934            plan,
935            rollouts,
936            weights,
937        })
938    }
939
940    #[allow(clippy::type_complexity)]
941    fn plan_step(
942        &mut self,
943        start: MppiState2D,
944        goal: (f64, f64),
945        keep_samples: bool,
946    ) -> RoboticsResult<(MppiPlan2D, Option<(Vec<MppiRollout2D>, Vec<f64>)>)> {
947        validate_state(start)?;
948        validate_goal(goal)?;
949        if let Some(gate_race) = &mut self.config.gate_race {
950            gate_race.advance_active_gate_from_state(start);
951        }
952        let normal = Normal::new(0.0, self.config.noise_sigma).map_err(|err| {
953            RoboticsError::InvalidParameter(format!("invalid MPPI noise distribution: {err}"))
954        })?;
955        let mut candidates = Vec::with_capacity(self.config.samples + 1);
956
957        candidates.push(self.rollout(start, goal, &self.nominal_controls));
958        for _ in 0..self.config.samples {
959            let mut controls = self.nominal_controls.clone();
960            for control in &mut controls {
961                control.ax = clamp_control(control.ax + normal.sample(&mut self.rng), &self.config);
962                control.ay = clamp_control(control.ay + normal.sample(&mut self.rng), &self.config);
963            }
964            candidates.push(self.rollout(start, goal, &controls));
965        }
966
967        let costs = candidates
968            .iter()
969            .map(|rollout| rollout.cost)
970            .collect::<Vec<_>>();
971        let sampling_lambda = select_sampling_lambda(&costs, &self.config)?;
972        let sampling_diagnostics = sampling_diagnostics_for_costs(&costs, sampling_lambda)?;
973        let beta = sampling_diagnostics.min_cost;
974        let mut weight_sum = 0.0;
975        let mut next_nominal = vec![MppiControl2D::new(0.0, 0.0); self.config.horizon];
976        let mut weights = Vec::with_capacity(if keep_samples { candidates.len() } else { 0 });
977        for rollout in &candidates {
978            let weight = sampling_weight(rollout.cost, beta, sampling_lambda);
979            weight_sum += weight;
980            if keep_samples {
981                weights.push(weight);
982            }
983            for (accumulator, control) in next_nominal.iter_mut().zip(&rollout.controls) {
984                accumulator.ax += weight * control.ax;
985                accumulator.ay += weight * control.ay;
986            }
987        }
988        if weight_sum <= 0.0 || !weight_sum.is_finite() {
989            return Err(RoboticsError::PlanningError(
990                "MPPI sample weights collapsed".to_string(),
991            ));
992        }
993        for control in &mut next_nominal {
994            control.ax = clamp_control(control.ax / weight_sum, &self.config);
995            control.ay = clamp_control(control.ay / weight_sum, &self.config);
996        }
997
998        let first_control = next_nominal[0];
999        self.nominal_controls = shift_controls(&next_nominal);
1000        let best_rollout = candidates
1001            .iter()
1002            .min_by(|a, b| a.cost.total_cmp(&b.cost))
1003            .expect("at least one candidate rollout")
1004            .clone();
1005        let samples = keep_samples.then(|| {
1006            weights.iter_mut().for_each(|weight| *weight /= weight_sum);
1007            (candidates, weights)
1008        });
1009
1010        Ok((
1011            MppiPlan2D {
1012                first_control,
1013                nominal_controls: next_nominal,
1014                best_rollout,
1015                sampling_diagnostics,
1016            },
1017            samples,
1018        ))
1019    }
1020
1021    pub fn terminal_value_grid(&self) -> Option<&MppiTerminalValueGrid2D> {
1022        self.config.terminal_value_grid.as_ref()
1023    }
1024
1025    pub fn terminal_value_grid_mut(&mut self) -> Option<&mut MppiTerminalValueGrid2D> {
1026        self.config.terminal_value_grid.as_mut()
1027    }
1028
1029    pub fn set_terminal_value_grid(
1030        &mut self,
1031        grid: Option<MppiTerminalValueGrid2D>,
1032    ) -> RoboticsResult<()> {
1033        if let Some(grid) = &grid {
1034            validate_terminal_value_grid(grid)?;
1035        }
1036        self.config.terminal_value_grid = grid;
1037        Ok(())
1038    }
1039
1040    pub fn set_moving_obstacles(
1041        &mut self,
1042        moving_obstacles: Vec<MppiMovingObstacle2D>,
1043    ) -> RoboticsResult<()> {
1044        validate_moving_obstacles(&moving_obstacles)?;
1045        self.config.moving_obstacles = moving_obstacles;
1046        Ok(())
1047    }
1048
1049    pub fn set_goal_trajectory(
1050        &mut self,
1051        goal_trajectory: Option<Vec<(f64, f64)>>,
1052    ) -> RoboticsResult<()> {
1053        if let Some(goal_trajectory) = &goal_trajectory {
1054            validate_goal_trajectory(goal_trajectory)?;
1055        }
1056        self.config.goal_trajectory = goal_trajectory;
1057        Ok(())
1058    }
1059
1060    fn rollout(
1061        &self,
1062        start: MppiState2D,
1063        goal: (f64, f64),
1064        controls: &[MppiControl2D],
1065    ) -> MppiRollout2D {
1066        let mut state = start;
1067        let mut states = Vec::with_capacity(controls.len() + 1);
1068        let mut model_cost = 0.0;
1069        let mut constraint_cost = 0.0;
1070        let mut max_constraint_violation: f64 = 0.0;
1071        let mut stage_costs = Vec::with_capacity(controls.len() + 1);
1072        let mut active_gate_index = self
1073            .config
1074            .gate_race
1075            .as_ref()
1076            .map_or(0, MppiGateRace2D::active_gate_index);
1077        states.push(state);
1078        for (step, &control) in controls.iter().enumerate() {
1079            let (step_constraint_cost, step_violation) = self.constraint_cost(state, step);
1080            constraint_cost += step_constraint_cost;
1081            max_constraint_violation = max_constraint_violation.max(step_violation);
1082            let next_state = state.step(control, self.config.dt);
1083            let gate_step_cost = if let Some(gate_race) = &self.config.gate_race {
1084                let gate_step = gate_race.transition_cost(state, next_state, active_gate_index);
1085                active_gate_index = gate_step.active_gate_index;
1086                gate_step.cost
1087            } else {
1088                0.0
1089            };
1090            let step_goal = self.goal_at(step, goal);
1091            let step_cost = self.running_cost(state, control, step_goal)
1092                + step_constraint_cost
1093                + gate_step_cost;
1094            model_cost += step_cost;
1095            stage_costs.push(step_cost);
1096            state = next_state;
1097            states.push(state);
1098        }
1099        let (terminal_constraint_cost, terminal_violation) =
1100            self.constraint_cost(state, controls.len());
1101        constraint_cost += terminal_constraint_cost;
1102        max_constraint_violation = max_constraint_violation.max(terminal_violation);
1103        let terminal_value_cost = self.terminal_value_cost(state);
1104        let terminal_gate_cost = self.config.gate_race.as_ref().map_or(0.0, |gate_race| {
1105            gate_race.terminal_cost(state, active_gate_index)
1106        });
1107        let terminal_goal = self.goal_at(controls.len(), goal);
1108        let terminal_model_cost = self.config.terminal_weight
1109            * squared_goal_distance(state, terminal_goal)
1110            + terminal_constraint_cost
1111            + terminal_gate_cost;
1112        model_cost += terminal_model_cost;
1113        stage_costs.push(terminal_model_cost);
1114        MppiRollout2D {
1115            cost: model_cost + terminal_value_cost,
1116            model_cost,
1117            constraint_cost,
1118            terminal_value_cost,
1119            max_constraint_violation,
1120            states,
1121            controls: controls.to_vec(),
1122            stage_costs,
1123        }
1124    }
1125
1126    fn goal_at(&self, step: usize, fallback_goal: (f64, f64)) -> (f64, f64) {
1127        self.config
1128            .goal_trajectory
1129            .as_ref()
1130            .and_then(|goals| goals.get(step).copied().or_else(|| goals.last().copied()))
1131            .unwrap_or(fallback_goal)
1132    }
1133
1134    fn running_cost(&self, state: MppiState2D, control: MppiControl2D, goal: (f64, f64)) -> f64 {
1135        self.config.goal_weight * squared_goal_distance(state, goal)
1136            + self.config.velocity_weight * (state.vx * state.vx + state.vy * state.vy)
1137            + self.config.control_weight * (control.ax * control.ax + control.ay * control.ay)
1138    }
1139
1140    fn constraint_cost(&self, state: MppiState2D, step: usize) -> (f64, f64) {
1141        let mut penalty = 0.0;
1142        let mut max_violation: f64 = 0.0;
1143        for obstacle in &self.config.obstacles {
1144            let violation =
1145                circular_obstacle_violation(state, *obstacle, self.config.safety_margin);
1146            max_violation = max_violation.max(violation);
1147            penalty += violation * violation;
1148        }
1149        let predicted_time = step as f64 * self.config.dt;
1150        for obstacle in &self.config.moving_obstacles {
1151            let predicted_obstacle = obstacle.predict(predicted_time);
1152            let violation =
1153                circular_obstacle_violation(state, predicted_obstacle, self.config.safety_margin);
1154            max_violation = max_violation.max(violation);
1155            penalty += violation * violation;
1156        }
1157        let discount = self.config.constraint_discount.powi(step as i32);
1158        (
1159            self.config.constraint_weight * discount * penalty,
1160            max_violation,
1161        )
1162    }
1163
1164    fn terminal_value_cost(&self, state: MppiState2D) -> f64 {
1165        self.config
1166            .terminal_value_grid
1167            .as_ref()
1168            .map_or(0.0, |grid| {
1169                self.config.terminal_value_weight * grid.value_at_state(state)
1170            })
1171    }
1172}
1173
1174fn validate_config(config: &MppiConfig) -> RoboticsResult<()> {
1175    if config.horizon == 0 || config.samples == 0 {
1176        return Err(RoboticsError::InvalidParameter(
1177            "horizon and samples must be positive".to_string(),
1178        ));
1179    }
1180    for (label, value) in [
1181        ("dt", config.dt),
1182        ("lambda", config.lambda),
1183        ("min_lambda", config.min_lambda),
1184        ("max_lambda", config.max_lambda),
1185        ("noise_sigma", config.noise_sigma),
1186        ("control_limit", config.control_limit),
1187    ] {
1188        if value <= 0.0 || !value.is_finite() {
1189            return Err(RoboticsError::InvalidParameter(format!(
1190                "{label} must be finite and positive"
1191            )));
1192        }
1193    }
1194    for (label, value) in [
1195        ("goal_weight", config.goal_weight),
1196        ("velocity_weight", config.velocity_weight),
1197        ("control_weight", config.control_weight),
1198        ("terminal_weight", config.terminal_weight),
1199        ("constraint_weight", config.constraint_weight),
1200        ("constraint_discount", config.constraint_discount),
1201        ("terminal_value_weight", config.terminal_value_weight),
1202    ] {
1203        if value < 0.0 || !value.is_finite() {
1204            return Err(RoboticsError::InvalidParameter(format!(
1205                "{label} must be finite and non-negative"
1206            )));
1207        }
1208    }
1209    if config.max_lambda < config.min_lambda {
1210        return Err(RoboticsError::InvalidParameter(
1211            "max_lambda must be at least min_lambda".to_string(),
1212        ));
1213    }
1214    if !(0.0..=1.0).contains(&config.target_effective_sample_ratio)
1215        || config.target_effective_sample_ratio == 0.0
1216    {
1217        return Err(RoboticsError::InvalidParameter(
1218            "target_effective_sample_ratio must be in (0, 1]".to_string(),
1219        ));
1220    }
1221    if config.adaptive_lambda_iterations == 0 {
1222        return Err(RoboticsError::InvalidParameter(
1223            "adaptive_lambda_iterations must be positive".to_string(),
1224        ));
1225    }
1226    if config.constraint_discount > 1.0 {
1227        return Err(RoboticsError::InvalidParameter(
1228            "constraint_discount must be at most 1.0".to_string(),
1229        ));
1230    }
1231    if config.safety_margin < 0.0 || !config.safety_margin.is_finite() {
1232        return Err(RoboticsError::InvalidParameter(
1233            "safety_margin must be finite and non-negative".to_string(),
1234        ));
1235    }
1236    for obstacle in &config.obstacles {
1237        if !obstacle.x.is_finite() || !obstacle.y.is_finite() {
1238            return Err(RoboticsError::InvalidParameter(
1239                "obstacle coordinates must be finite".to_string(),
1240            ));
1241        }
1242        if obstacle.radius <= 0.0 || !obstacle.radius.is_finite() {
1243            return Err(RoboticsError::InvalidParameter(
1244                "obstacle radius must be finite and positive".to_string(),
1245            ));
1246        }
1247    }
1248    validate_moving_obstacles(&config.moving_obstacles)?;
1249    if let Some(goal_trajectory) = &config.goal_trajectory {
1250        validate_goal_trajectory(goal_trajectory)?;
1251    }
1252    if let Some(grid) = &config.terminal_value_grid {
1253        validate_terminal_value_grid(grid)?;
1254    }
1255    if let Some(gate_race) = &config.gate_race {
1256        validate_gate_race(gate_race)?;
1257    }
1258    Ok(())
1259}
1260
1261fn validate_moving_obstacles(obstacles: &[MppiMovingObstacle2D]) -> RoboticsResult<()> {
1262    for obstacle in obstacles {
1263        if !obstacle.x.is_finite()
1264            || !obstacle.y.is_finite()
1265            || !obstacle.vx.is_finite()
1266            || !obstacle.vy.is_finite()
1267        {
1268            return Err(RoboticsError::InvalidParameter(
1269                "moving obstacle state must be finite".to_string(),
1270            ));
1271        }
1272        if obstacle.radius <= 0.0 || !obstacle.radius.is_finite() {
1273            return Err(RoboticsError::InvalidParameter(
1274                "moving obstacle radius must be finite and positive".to_string(),
1275            ));
1276        }
1277    }
1278    Ok(())
1279}
1280
1281fn validate_goal_trajectory(goal_trajectory: &[(f64, f64)]) -> RoboticsResult<()> {
1282    if goal_trajectory.is_empty() {
1283        return Err(RoboticsError::InvalidParameter(
1284            "MPPI goal trajectory must be non-empty".to_string(),
1285        ));
1286    }
1287    for &goal in goal_trajectory {
1288        validate_goal(goal)?;
1289    }
1290    Ok(())
1291}
1292
1293fn validate_gate_race(gate_race: &MppiGateRace2D) -> RoboticsResult<()> {
1294    if gate_race.gates.is_empty() {
1295        return Err(RoboticsError::InvalidParameter(
1296            "gate race must contain at least one gate".to_string(),
1297        ));
1298    }
1299    if gate_race.active_gate_index > gate_race.gates.len() {
1300        return Err(RoboticsError::InvalidParameter(
1301            "active gate index must be within the gate list".to_string(),
1302        ));
1303    }
1304    for (label, value) in [
1305        ("gate race progress_weight", gate_race.progress_weight),
1306        ("gate race pass_bonus", gate_race.pass_bonus),
1307        ("gate race miss_penalty", gate_race.miss_penalty),
1308        (
1309            "gate race terminal_gate_weight",
1310            gate_race.terminal_gate_weight,
1311        ),
1312    ] {
1313        if value <= 0.0 || !value.is_finite() {
1314            return Err(RoboticsError::InvalidParameter(format!(
1315                "{label} must be finite and positive"
1316            )));
1317        }
1318    }
1319    for (label, value) in [
1320        ("gate race lateral_weight", gate_race.lateral_weight),
1321        ("gate race crossing_tolerance", gate_race.crossing_tolerance),
1322    ] {
1323        if value < 0.0 || !value.is_finite() {
1324            return Err(RoboticsError::InvalidParameter(format!(
1325                "{label} must be finite and non-negative"
1326            )));
1327        }
1328    }
1329    for gate in &gate_race.gates {
1330        let normal_norm = (gate.normal_x * gate.normal_x + gate.normal_y * gate.normal_y).sqrt();
1331        if !gate.center_x.is_finite()
1332            || !gate.center_y.is_finite()
1333            || !gate.normal_x.is_finite()
1334            || !gate.normal_y.is_finite()
1335            || normal_norm <= 0.0
1336        {
1337            return Err(RoboticsError::InvalidParameter(
1338                "racing gate center and normal must be finite".to_string(),
1339            ));
1340        }
1341        if gate.half_width <= 0.0 || !gate.half_width.is_finite() {
1342            return Err(RoboticsError::InvalidParameter(
1343                "racing gate half_width must be finite and positive".to_string(),
1344            ));
1345        }
1346    }
1347    Ok(())
1348}
1349
1350fn validate_track_waypoints(waypoints: &[(f64, f64)]) -> RoboticsResult<()> {
1351    if waypoints.len() < 2 {
1352        return Err(RoboticsError::InvalidParameter(
1353            "track must contain at least two waypoints".to_string(),
1354        ));
1355    }
1356    for &(x, y) in waypoints {
1357        if !x.is_finite() || !y.is_finite() {
1358            return Err(RoboticsError::InvalidParameter(
1359                "track waypoints must be finite".to_string(),
1360            ));
1361        }
1362    }
1363    for pair in waypoints.windows(2) {
1364        if point_distance(pair[0], pair[1]) <= 0.0 {
1365            return Err(RoboticsError::InvalidParameter(
1366                "track segments must have positive length".to_string(),
1367            ));
1368        }
1369    }
1370    Ok(())
1371}
1372
1373fn point_distance(a: (f64, f64), b: (f64, f64)) -> f64 {
1374    let dx = a.0 - b.0;
1375    let dy = a.1 - b.1;
1376    (dx * dx + dy * dy).sqrt()
1377}
1378
1379fn sampling_weight(cost: f64, beta: f64, lambda: f64) -> f64 {
1380    (-(cost - beta) / lambda).exp()
1381}
1382
1383fn sampling_diagnostics_for_costs(
1384    costs: &[f64],
1385    lambda: f64,
1386) -> RoboticsResult<MppiSamplingDiagnostics2D> {
1387    if costs.is_empty() {
1388        return Err(RoboticsError::InvalidParameter(
1389            "MPPI sampling costs must be non-empty".to_string(),
1390        ));
1391    }
1392    if lambda <= 0.0 || !lambda.is_finite() {
1393        return Err(RoboticsError::InvalidParameter(
1394            "MPPI sampling lambda must be finite and positive".to_string(),
1395        ));
1396    }
1397    if costs.iter().any(|cost| !cost.is_finite()) {
1398        return Err(RoboticsError::InvalidParameter(
1399            "MPPI sampling costs must be finite".to_string(),
1400        ));
1401    }
1402
1403    let sample_count = costs.len();
1404    let min_cost = costs.iter().copied().fold(f64::INFINITY, f64::min);
1405    let max_cost = costs.iter().copied().fold(f64::NEG_INFINITY, f64::max);
1406    let mean_cost = costs.iter().sum::<f64>() / sample_count as f64;
1407    let mut weight_sum = 0.0;
1408    let mut squared_weight_sum = 0.0;
1409    let mut weights = Vec::with_capacity(sample_count);
1410    for &cost in costs {
1411        let weight = sampling_weight(cost, min_cost, lambda);
1412        weight_sum += weight;
1413        squared_weight_sum += weight * weight;
1414        weights.push(weight);
1415    }
1416    if weight_sum <= 0.0 || squared_weight_sum <= 0.0 || !weight_sum.is_finite() {
1417        return Err(RoboticsError::PlanningError(
1418            "MPPI sample weights collapsed".to_string(),
1419        ));
1420    }
1421
1422    let effective_sample_size = weight_sum * weight_sum / squared_weight_sum;
1423    let mut weight_entropy = 0.0;
1424    for weight in weights {
1425        let probability = weight / weight_sum;
1426        if probability > 0.0 {
1427            weight_entropy -= probability * probability.ln();
1428        }
1429    }
1430    let sample_count_f = sample_count as f64;
1431    let normalized_weight_entropy = if sample_count > 1 {
1432        weight_entropy / sample_count_f.ln()
1433    } else {
1434        1.0
1435    };
1436
1437    Ok(MppiSamplingDiagnostics2D {
1438        sample_count,
1439        lambda,
1440        min_cost,
1441        mean_cost,
1442        max_cost,
1443        effective_sample_size,
1444        normalized_effective_sample_size: effective_sample_size / sample_count_f,
1445        weight_entropy,
1446        normalized_weight_entropy,
1447    })
1448}
1449
1450fn select_sampling_lambda(costs: &[f64], config: &MppiConfig) -> RoboticsResult<f64> {
1451    if !config.adaptive_lambda {
1452        return Ok(config.lambda);
1453    }
1454
1455    let target_effective_sample_size = config.target_effective_sample_ratio * costs.len() as f64;
1456    let min_diag = sampling_diagnostics_for_costs(costs, config.min_lambda)?;
1457    if min_diag.effective_sample_size >= target_effective_sample_size {
1458        return Ok(config.min_lambda);
1459    }
1460    let max_diag = sampling_diagnostics_for_costs(costs, config.max_lambda)?;
1461    if max_diag.effective_sample_size <= target_effective_sample_size {
1462        return Ok(config.max_lambda);
1463    }
1464
1465    let mut low = config.min_lambda;
1466    let mut high = config.max_lambda;
1467    for _ in 0..config.adaptive_lambda_iterations {
1468        let mid = 0.5 * (low + high);
1469        let diagnostics = sampling_diagnostics_for_costs(costs, mid)?;
1470        if diagnostics.effective_sample_size < target_effective_sample_size {
1471            low = mid;
1472        } else {
1473            high = mid;
1474        }
1475    }
1476    Ok(high)
1477}
1478
1479fn validate_terminal_value_grid(grid: &MppiTerminalValueGrid2D) -> RoboticsResult<()> {
1480    if !grid.origin_x.is_finite() || !grid.origin_y.is_finite() {
1481        return Err(RoboticsError::InvalidParameter(
1482            "terminal value grid origin must be finite".to_string(),
1483        ));
1484    }
1485    if grid.resolution <= 0.0 || !grid.resolution.is_finite() {
1486        return Err(RoboticsError::InvalidParameter(
1487            "terminal value grid resolution must be finite and positive".to_string(),
1488        ));
1489    }
1490    if grid.values.is_empty() || grid.values[0].is_empty() {
1491        return Err(RoboticsError::InvalidParameter(
1492            "terminal value grid values must be non-empty".to_string(),
1493        ));
1494    }
1495    let height = grid.values[0].len();
1496    for column in &grid.values {
1497        if column.len() != height {
1498            return Err(RoboticsError::InvalidParameter(
1499                "terminal value grid must be rectangular".to_string(),
1500            ));
1501        }
1502        if column
1503            .iter()
1504            .any(|value| !value.is_finite() || *value < 0.0)
1505        {
1506            return Err(RoboticsError::InvalidParameter(
1507                "terminal value grid values must be finite and non-negative".to_string(),
1508            ));
1509        }
1510    }
1511    Ok(())
1512}
1513
1514fn validate_terminal_value_update_config(
1515    config: &MppiTerminalValueUpdateConfig2D,
1516) -> RoboticsResult<()> {
1517    if !(0.0..=1.0).contains(&config.learning_rate) || config.learning_rate == 0.0 {
1518        return Err(RoboticsError::InvalidParameter(
1519            "terminal value update learning_rate must be in (0, 1]".to_string(),
1520        ));
1521    }
1522    if !(0.0..=1.0).contains(&config.discount) {
1523        return Err(RoboticsError::InvalidParameter(
1524            "terminal value update discount must be in [0, 1]".to_string(),
1525        ));
1526    }
1527    Ok(())
1528}
1529
1530fn validate_rollout_for_terminal_value_update(rollout: &MppiRollout2D) -> RoboticsResult<()> {
1531    if rollout.states.is_empty() || rollout.stage_costs.is_empty() {
1532        return Err(RoboticsError::InvalidParameter(
1533            "rollout states and stage_costs must be non-empty".to_string(),
1534        ));
1535    }
1536    if rollout.states.len() != rollout.stage_costs.len() {
1537        return Err(RoboticsError::InvalidParameter(
1538            "rollout states and stage_costs must have the same length".to_string(),
1539        ));
1540    }
1541    for &state in &rollout.states {
1542        validate_state(state)?;
1543    }
1544    if rollout
1545        .stage_costs
1546        .iter()
1547        .any(|value| !value.is_finite() || *value < 0.0)
1548    {
1549        return Err(RoboticsError::InvalidParameter(
1550            "rollout stage_costs must be finite and non-negative".to_string(),
1551        ));
1552    }
1553    Ok(())
1554}
1555
1556fn discounted_cost_to_go(stage_costs: &[f64], discount: f64) -> Vec<f64> {
1557    let mut targets = vec![0.0; stage_costs.len()];
1558    let mut target = 0.0;
1559    for index in (0..stage_costs.len()).rev() {
1560        target = stage_costs[index] + discount * target;
1561        targets[index] = target;
1562    }
1563    targets
1564}
1565
1566fn validate_state(state: MppiState2D) -> RoboticsResult<()> {
1567    for (label, value) in [
1568        ("x", state.x),
1569        ("y", state.y),
1570        ("vx", state.vx),
1571        ("vy", state.vy),
1572    ] {
1573        if !value.is_finite() {
1574            return Err(RoboticsError::InvalidParameter(format!(
1575                "{label} must be finite"
1576            )));
1577        }
1578    }
1579    Ok(())
1580}
1581
1582fn validate_goal(goal: (f64, f64)) -> RoboticsResult<()> {
1583    if goal.0.is_finite() && goal.1.is_finite() {
1584        Ok(())
1585    } else {
1586        Err(RoboticsError::InvalidParameter(
1587            "goal coordinates must be finite".to_string(),
1588        ))
1589    }
1590}
1591
1592fn squared_goal_distance(state: MppiState2D, goal: (f64, f64)) -> f64 {
1593    let dx = state.x - goal.0;
1594    let dy = state.y - goal.1;
1595    dx * dx + dy * dy
1596}
1597
1598fn circular_obstacle_violation(
1599    state: MppiState2D,
1600    obstacle: MppiCircularObstacle2D,
1601    safety_margin: f64,
1602) -> f64 {
1603    let dx = state.x - obstacle.x;
1604    let dy = state.y - obstacle.y;
1605    let clearance = (dx * dx + dy * dy).sqrt() - obstacle.radius - safety_margin;
1606    (-clearance).max(0.0)
1607}
1608
1609fn clamp_control(value: f64, config: &MppiConfig) -> f64 {
1610    value.clamp(-config.control_limit, config.control_limit)
1611}
1612
1613fn shift_controls(controls: &[MppiControl2D]) -> Vec<MppiControl2D> {
1614    let mut shifted = controls[1..].to_vec();
1615    shifted.push(MppiControl2D::new(0.0, 0.0));
1616    shifted
1617}
1618
1619#[cfg(test)]
1620mod tests {
1621    use super::*;
1622
1623    fn small_config(seed: u64) -> MppiConfig {
1624        MppiConfig {
1625            horizon: 12,
1626            samples: 64,
1627            dt: 0.1,
1628            seed,
1629            ..MppiConfig::default()
1630        }
1631    }
1632
1633    #[test]
1634    fn mppi_accelerates_toward_goal() {
1635        let mut controller = MppiController2D::new(small_config(3)).unwrap();
1636        let plan = controller
1637            .plan(MppiState2D::new(0.0, 0.0, 0.0, 0.0), (2.0, 0.0))
1638            .unwrap();
1639
1640        assert!(plan.first_control.ax > 0.0);
1641        assert!(plan.first_control.ay.abs() < 0.8);
1642        assert_eq!(plan.nominal_controls.len(), 12);
1643    }
1644
1645    #[test]
1646    fn rollout_best_cost_is_finite() {
1647        let mut controller = MppiController2D::new(small_config(4)).unwrap();
1648        let plan = controller
1649            .plan(MppiState2D::new(0.0, 0.0, 0.0, 0.0), (1.0, 1.0))
1650            .unwrap();
1651
1652        assert!(plan.best_rollout.cost.is_finite());
1653        assert_eq!(plan.best_rollout.states.len(), 13);
1654    }
1655
1656    #[test]
1657    fn plan_reports_sampling_diagnostics() {
1658        let mut controller = MppiController2D::new(small_config(8)).unwrap();
1659        let plan = controller
1660            .plan(MppiState2D::new(0.0, 0.0, 0.0, 0.0), (1.5, 0.0))
1661            .unwrap();
1662
1663        let diagnostics = plan.sampling_diagnostics;
1664        assert_eq!(diagnostics.sample_count, 65);
1665        assert_eq!(diagnostics.lambda, 1.0);
1666        assert!(diagnostics.min_cost <= diagnostics.mean_cost);
1667        assert!(diagnostics.mean_cost <= diagnostics.max_cost);
1668        assert!(diagnostics.effective_sample_size >= 1.0);
1669        assert!(diagnostics.effective_sample_size <= diagnostics.sample_count as f64);
1670        assert!(diagnostics.normalized_effective_sample_size > 0.0);
1671        assert!(diagnostics.normalized_effective_sample_size <= 1.0);
1672        assert!(diagnostics.normalized_weight_entropy >= 0.0);
1673        assert!(diagnostics.normalized_weight_entropy <= 1.0);
1674    }
1675
1676    #[test]
1677    fn adaptive_lambda_raises_effective_sample_size() {
1678        let costs = vec![0.0, 10.0, 20.0, 30.0, 40.0];
1679        let fixed = sampling_diagnostics_for_costs(&costs, 0.05).unwrap();
1680        let config = MppiConfig {
1681            adaptive_lambda: true,
1682            min_lambda: 0.05,
1683            max_lambda: 100.0,
1684            target_effective_sample_ratio: 0.6,
1685            adaptive_lambda_iterations: 32,
1686            ..MppiConfig::default()
1687        };
1688        let selected_lambda = select_sampling_lambda(&costs, &config).unwrap();
1689        let adaptive = sampling_diagnostics_for_costs(&costs, selected_lambda).unwrap();
1690
1691        assert!(selected_lambda > config.min_lambda);
1692        assert!(adaptive.effective_sample_size > fixed.effective_sample_size);
1693        assert!(adaptive.normalized_effective_sample_size >= 0.55);
1694    }
1695
1696    #[test]
1697    fn plan_with_samples_is_plan_plus_the_sample_cloud() {
1698        let config = MppiConfig {
1699            samples: 64,
1700            obstacles: vec![MppiCircularObstacle2D::new(2.0, 0.2, 0.6)],
1701            ..MppiConfig::default()
1702        };
1703        let mut plain = MppiController2D::new(config.clone()).unwrap();
1704        let mut sampled = MppiController2D::new(config).unwrap();
1705        let mut start = MppiState2D::new(0.0, 0.0, 0.0, 0.0);
1706        for _ in 0..3 {
1707            let plan = plain.plan(start, (4.0, 0.0)).unwrap();
1708            let cloud = sampled.plan_with_samples(start, (4.0, 0.0)).unwrap();
1709            assert_eq!(plan, cloud.plan);
1710            assert_eq!(cloud.rollouts.len(), 65);
1711            assert_eq!(cloud.weights.len(), 65);
1712            assert!((cloud.weights.iter().sum::<f64>() - 1.0).abs() < 1e-9);
1713            let heaviest = cloud
1714                .weights
1715                .iter()
1716                .enumerate()
1717                .max_by(|a, b| a.1.total_cmp(b.1))
1718                .unwrap()
1719                .0;
1720            assert_eq!(cloud.rollouts[heaviest], cloud.plan.best_rollout);
1721            start = start.step(plan.first_control, 0.1);
1722        }
1723    }
1724
1725    #[test]
1726    fn circular_obstacle_violation_respects_safety_margin() {
1727        let obstacle = MppiCircularObstacle2D::new(1.0, 0.0, 0.5);
1728
1729        let outside =
1730            circular_obstacle_violation(MppiState2D::new(2.0, 0.0, 0.0, 0.0), obstacle, 0.1);
1731        let inside =
1732            circular_obstacle_violation(MppiState2D::new(1.2, 0.0, 0.0, 0.0), obstacle, 0.1);
1733
1734        assert_eq!(outside, 0.0);
1735        assert!((inside - 0.4).abs() < 1e-9);
1736    }
1737
1738    #[test]
1739    fn constraint_cost_is_discounted_over_horizon() {
1740        let config = MppiConfig {
1741            obstacles: vec![MppiCircularObstacle2D::new(1.0, 0.0, 0.5)],
1742            constraint_weight: 10.0,
1743            constraint_discount: 0.5,
1744            safety_margin: 0.0,
1745            ..small_config(5)
1746        };
1747        let controller = MppiController2D::new(config).unwrap();
1748        let state = MppiState2D::new(1.25, 0.0, 0.0, 0.0);
1749
1750        let (early_cost, early_violation) = controller.constraint_cost(state, 0);
1751        let (late_cost, late_violation) = controller.constraint_cost(state, 3);
1752
1753        assert!((early_violation - 0.25).abs() < 1e-9);
1754        assert_eq!(early_violation, late_violation);
1755        assert!((early_cost - 0.625).abs() < 1e-9);
1756        assert!((late_cost - 0.078125).abs() < 1e-9);
1757    }
1758
1759    #[test]
1760    fn rollout_reports_constraint_cost_and_violation() {
1761        let config = MppiConfig {
1762            obstacles: vec![MppiCircularObstacle2D::new(0.0, 0.0, 0.5)],
1763            constraint_weight: 10.0,
1764            safety_margin: 0.0,
1765            ..small_config(6)
1766        };
1767        let controller = MppiController2D::new(config).unwrap();
1768        let controls = vec![MppiControl2D::new(0.0, 0.0); 12];
1769        let rollout =
1770            controller.rollout(MppiState2D::new(0.0, 0.0, 0.0, 0.0), (1.0, 0.0), &controls);
1771
1772        assert!(rollout.constraint_cost > 0.0);
1773        assert!((rollout.max_constraint_violation - 0.5).abs() < 1e-9);
1774    }
1775
1776    #[test]
1777    fn terminal_value_grid_interpolates_and_clamps() {
1778        let grid =
1779            MppiTerminalValueGrid2D::new(0.0, 0.0, 1.0, vec![vec![0.0, 1.0], vec![1.0, 2.0]])
1780                .unwrap();
1781
1782        assert!((grid.value_at(0.5, 0.5) - 1.0).abs() < 1e-9);
1783        assert!((grid.value_at(-10.0, 0.5) - 0.5).abs() < 1e-9);
1784        assert!((grid.value_at(10.0, 10.0) - 2.0).abs() < 1e-9);
1785    }
1786
1787    #[test]
1788    fn waypoint_track_projects_points_to_progress() {
1789        let track = MppiWaypointTrack2D::new(vec![(0.0, 0.0), (1.0, 0.0), (1.0, 1.0)]).unwrap();
1790
1791        let first = track.project(0.5, 0.2).unwrap();
1792        let second = track.project(1.2, 0.6).unwrap();
1793
1794        assert_eq!(first.segment_index, 0);
1795        assert!((first.progress - 0.5).abs() < 1e-9);
1796        assert!((first.lateral_error - 0.2).abs() < 1e-9);
1797        assert_eq!(second.segment_index, 1);
1798        assert!((second.progress - 1.6).abs() < 1e-9);
1799        assert!((track.total_length() - 2.0).abs() < 1e-9);
1800    }
1801
1802    #[test]
1803    fn waypoint_track_returns_point_at_progress() {
1804        let track = MppiWaypointTrack2D::new(vec![(0.0, 0.0), (1.0, 0.0), (1.0, 2.0)]).unwrap();
1805
1806        let first = track.point_at_progress(0.5).unwrap();
1807        let second = track.point_at_progress(2.0).unwrap();
1808        let clamped = track.point_at_progress(10.0).unwrap();
1809
1810        assert!((first.0 - 0.5).abs() < 1e-9);
1811        assert!((first.1 - 0.0).abs() < 1e-9);
1812        assert!((second.0 - 1.0).abs() < 1e-9);
1813        assert!((second.1 - 1.0).abs() < 1e-9);
1814        assert_eq!(clamped, track.goal());
1815    }
1816
1817    #[test]
1818    fn waypoint_track_terminal_grid_prefers_progress_and_low_lateral_error() {
1819        let track = MppiWaypointTrack2D::new(vec![(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)]).unwrap();
1820        let grid = track
1821            .terminal_value_grid(5, 5, 0.0, -1.0, 0.5, 1.0, 2.0)
1822            .unwrap();
1823
1824        let start_value = grid.value_at(0.0, 0.0);
1825        let middle_value = grid.value_at(1.0, 0.0);
1826        let goal_value = grid.value_at(2.0, 0.0);
1827        let off_track_value = grid.value_at(1.0, 1.0);
1828
1829        assert!(start_value > middle_value);
1830        assert!(middle_value > goal_value);
1831        assert!(off_track_value > middle_value);
1832    }
1833
1834    #[test]
1835    fn moving_obstacle_is_predicted_in_constraint_cost() {
1836        let config = MppiConfig {
1837            moving_obstacles: vec![MppiMovingObstacle2D::new(0.0, 0.0, 1.0, 0.0, 0.4)],
1838            constraint_weight: 10.0,
1839            safety_margin: 0.0,
1840            ..small_config(5)
1841        };
1842        let controller = MppiController2D::new(config).unwrap();
1843        let state = MppiState2D::new(0.2, 0.0, 0.0, 0.0);
1844        let future_state = MppiState2D::new(0.5, 0.0, 0.0, 0.0);
1845
1846        let (early_cost, early_violation) = controller.constraint_cost(state, 0);
1847        let (future_cost, future_violation) = controller.constraint_cost(future_state, 5);
1848
1849        assert!(early_cost > 0.0);
1850        assert!((early_violation - 0.2).abs() < 1e-9);
1851        assert!(future_cost > 0.0);
1852        assert!((future_violation - 0.4).abs() < 1e-9);
1853    }
1854
1855    #[test]
1856    fn rollout_uses_horizon_goal_trajectory_when_present() {
1857        let config = MppiConfig {
1858            horizon: 1,
1859            samples: 1,
1860            goal_weight: 1.0,
1861            velocity_weight: 0.0,
1862            control_weight: 0.0,
1863            terminal_weight: 1.0,
1864            goal_trajectory: Some(vec![(0.0, 0.0), (2.0, 0.0)]),
1865            ..MppiConfig::default()
1866        };
1867        let controller = MppiController2D::new(config).unwrap();
1868        let controls = vec![MppiControl2D::new(0.0, 0.0)];
1869        let rollout =
1870            controller.rollout(MppiState2D::new(0.0, 0.0, 0.0, 0.0), (10.0, 0.0), &controls);
1871
1872        assert!((rollout.stage_costs[0] - 0.0).abs() < 1e-9);
1873        assert!((rollout.stage_costs[1] - 4.0).abs() < 1e-9);
1874        assert!((rollout.model_cost - 4.0).abs() < 1e-9);
1875    }
1876
1877    #[test]
1878    fn racing_gate_normalizes_and_detects_crossing() {
1879        let gate = MppiRacingGate2D::new(1.0, 0.0, 2.0, 0.0, 0.35).unwrap();
1880        let from = MppiState2D::new(0.8, 0.1, 0.0, 0.0);
1881        let through = MppiState2D::new(1.2, 0.1, 0.0, 0.0);
1882        let wide = MppiState2D::new(1.2, 0.8, 0.0, 0.0);
1883
1884        assert!((gate.normal_x - 1.0).abs() < 1e-9);
1885        assert!(gate.crosses(from, through, 0.0));
1886        assert!(!gate.crosses(from, wide, 0.0));
1887        assert!(gate.state_is_past_gate(through, 0.0));
1888        assert!(!gate.state_is_past_gate(wide, 0.0));
1889    }
1890
1891    #[test]
1892    fn gate_race_scores_progress_and_switches_gates() {
1893        let race = MppiGateRace2D::new(vec![
1894            MppiRacingGate2D::new(1.0, 0.0, 1.0, 0.0, 0.35).unwrap(),
1895            MppiRacingGate2D::new(2.0, 0.0, 1.0, 0.0, 0.35).unwrap(),
1896        ])
1897        .unwrap();
1898        let states = vec![
1899            MppiState2D::new(0.0, 0.0, 0.0, 0.0),
1900            MppiState2D::new(1.1, 0.0, 0.0, 0.0),
1901            MppiState2D::new(2.1, 0.0, 0.0, 0.0),
1902        ];
1903
1904        let report = race.score_trajectory(&states).unwrap();
1905
1906        assert_eq!(report.passed_gates, 2);
1907        assert_eq!(report.active_gate_index, 2);
1908        assert_eq!(report.final_gate_distance, 0.0);
1909        assert!(report.cost < 0.0);
1910    }
1911
1912    #[test]
1913    fn gate_race_rollout_can_run_without_goal_tracking_cost() {
1914        let race = MppiGateRace2D::new(vec![
1915            MppiRacingGate2D::new(0.01, 0.0, 1.0, 0.0, 0.3).unwrap()
1916        ])
1917        .unwrap();
1918        let config = MppiConfig {
1919            horizon: 1,
1920            samples: 1,
1921            goal_weight: 0.0,
1922            velocity_weight: 0.0,
1923            control_weight: 0.0,
1924            terminal_weight: 0.0,
1925            gate_race: Some(race),
1926            ..MppiConfig::default()
1927        };
1928        let controller = MppiController2D::new(config).unwrap();
1929        let controls = vec![MppiControl2D::new(2.0, 0.0)];
1930        let rollout = controller.rollout(
1931            MppiState2D::new(0.0, 0.0, 0.0, 0.0),
1932            (10.0, 10.0),
1933            &controls,
1934        );
1935
1936        assert!(rollout.cost < 0.0);
1937        assert_eq!(rollout.constraint_cost, 0.0);
1938        assert_eq!(rollout.terminal_value_cost, 0.0);
1939    }
1940
1941    #[test]
1942    fn waypoint_track_rejects_invalid_waypoints() {
1943        assert!(MppiWaypointTrack2D::new(vec![(0.0, 0.0)]).is_err());
1944        assert!(MppiWaypointTrack2D::new(vec![(0.0, 0.0), (0.0, 0.0)]).is_err());
1945        assert!(MppiWaypointTrack2D::new(vec![(0.0, 0.0), (f64::NAN, 1.0)]).is_err());
1946    }
1947
1948    #[test]
1949    fn terminal_value_cost_is_added_to_rollout() {
1950        let grid = MppiTerminalValueGrid2D::new(0.0, 0.0, 1.0, vec![vec![3.0]]).unwrap();
1951        let config = MppiConfig {
1952            horizon: 1,
1953            samples: 1,
1954            terminal_value_weight: 2.0,
1955            terminal_value_grid: Some(grid),
1956            ..MppiConfig::default()
1957        };
1958        let controller = MppiController2D::new(config).unwrap();
1959        let controls = vec![MppiControl2D::new(0.0, 0.0)];
1960        let rollout =
1961            controller.rollout(MppiState2D::new(0.0, 0.0, 0.0, 0.0), (0.0, 0.0), &controls);
1962
1963        assert!((rollout.terminal_value_cost - 6.0).abs() < 1e-9);
1964        assert!((rollout.cost - 6.0).abs() < 1e-9);
1965        assert!((rollout.model_cost - 0.0).abs() < 1e-9);
1966        assert!((rollout.cost - rollout.model_cost - rollout.terminal_value_cost).abs() < 1e-9);
1967        assert_eq!(rollout.stage_costs.len(), rollout.states.len());
1968    }
1969
1970    #[test]
1971    fn rejects_invalid_terminal_value_grid() {
1972        let ragged = MppiTerminalValueGrid2D::new(0.0, 0.0, 1.0, vec![vec![1.0], vec![]]);
1973        assert!(matches!(ragged, Err(RoboticsError::InvalidParameter(_))));
1974
1975        let negative = MppiTerminalValueGrid2D::new(0.0, 0.0, 1.0, vec![vec![-1.0]]);
1976        assert!(matches!(negative, Err(RoboticsError::InvalidParameter(_))));
1977    }
1978
1979    #[test]
1980    fn terminal_value_updater_learns_discounted_rollout_costs() {
1981        let mut grid = MppiTerminalValueGrid2D::new(0.0, 0.0, 1.0, vec![vec![0.0, 0.0]]).unwrap();
1982        let updater = MppiTerminalValueUpdater2D::new(MppiTerminalValueUpdateConfig2D {
1983            learning_rate: 1.0,
1984            discount: 0.5,
1985        })
1986        .unwrap();
1987        let rollout = MppiRollout2D {
1988            cost: 3.0,
1989            model_cost: 3.0,
1990            constraint_cost: 0.0,
1991            terminal_value_cost: 0.0,
1992            max_constraint_violation: 0.0,
1993            states: vec![
1994                MppiState2D::new(0.0, 0.0, 0.0, 0.0),
1995                MppiState2D::new(0.0, 1.0, 0.0, 0.0),
1996            ],
1997            controls: vec![MppiControl2D::new(0.0, 0.0)],
1998            stage_costs: vec![2.0, 1.0],
1999        };
2000
2001        let report = updater.update_from_rollout(&mut grid, &rollout).unwrap();
2002
2003        assert_eq!(report.updates, 2);
2004        assert!((report.start_target - 2.5).abs() < 1e-9);
2005        assert!((report.terminal_target - 1.0).abs() < 1e-9);
2006        assert!((grid.value_at_cell(0, 0).unwrap() - 2.5).abs() < 1e-9);
2007        assert!((grid.value_at_cell(0, 1).unwrap() - 1.0).abs() < 1e-9);
2008    }
2009
2010    #[test]
2011    fn terminal_value_updater_rejects_mismatched_rollout() {
2012        let mut grid = MppiTerminalValueGrid2D::new(0.0, 0.0, 1.0, vec![vec![0.0]]).unwrap();
2013        let updater =
2014            MppiTerminalValueUpdater2D::new(MppiTerminalValueUpdateConfig2D::default()).unwrap();
2015        let rollout = MppiRollout2D {
2016            cost: 0.0,
2017            model_cost: 0.0,
2018            constraint_cost: 0.0,
2019            terminal_value_cost: 0.0,
2020            max_constraint_violation: 0.0,
2021            states: vec![MppiState2D::new(0.0, 0.0, 0.0, 0.0)],
2022            controls: Vec::new(),
2023            stage_costs: Vec::new(),
2024        };
2025
2026        assert!(matches!(
2027            updater.update_from_rollout(&mut grid, &rollout),
2028            Err(RoboticsError::InvalidParameter(_))
2029        ));
2030    }
2031
2032    fn synthetic_terminal_rollout(y: f64, stage_cost: f64) -> MppiRollout2D {
2033        MppiRollout2D {
2034            cost: stage_cost,
2035            model_cost: stage_cost,
2036            constraint_cost: 0.0,
2037            terminal_value_cost: 0.0,
2038            max_constraint_violation: 0.0,
2039            states: vec![MppiState2D::new(0.0, y, 0.0, 0.0)],
2040            controls: Vec::new(),
2041            stage_costs: vec![stage_cost],
2042        }
2043    }
2044
2045    #[test]
2046    fn terminal_value_replay_buffer_caps_capacity_and_updates_grid() {
2047        let updater = MppiTerminalValueUpdater2D::new(MppiTerminalValueUpdateConfig2D {
2048            learning_rate: 1.0,
2049            discount: 0.0,
2050        })
2051        .unwrap();
2052        let mut buffer = MppiTerminalValueReplayBuffer2D::new(2).unwrap();
2053        buffer.push(synthetic_terminal_rollout(0.0, 9.0)).unwrap();
2054        buffer.push(synthetic_terminal_rollout(1.0, 2.0)).unwrap();
2055        buffer.push(synthetic_terminal_rollout(2.0, 3.0)).unwrap();
2056
2057        let mut grid =
2058            MppiTerminalValueGrid2D::new(0.0, 0.0, 1.0, vec![vec![0.0, 0.0, 0.0]]).unwrap();
2059        let report = buffer.update_grid(&mut grid, &updater).unwrap();
2060
2061        assert_eq!(buffer.capacity(), 2);
2062        assert_eq!(buffer.len(), 2);
2063        assert_eq!(report.rollouts, 2);
2064        assert_eq!(report.updates, 2);
2065        assert_eq!(grid.value_at_cell(0, 0).unwrap(), 0.0);
2066        assert_eq!(grid.value_at_cell(0, 1).unwrap(), 2.0);
2067        assert_eq!(grid.value_at_cell(0, 2).unwrap(), 3.0);
2068    }
2069
2070    #[test]
2071    fn terminal_value_replay_buffer_rejects_empty_updates() {
2072        let updater =
2073            MppiTerminalValueUpdater2D::new(MppiTerminalValueUpdateConfig2D::default()).unwrap();
2074        let buffer = MppiTerminalValueReplayBuffer2D::new(2).unwrap();
2075        let mut grid = MppiTerminalValueGrid2D::new(0.0, 0.0, 1.0, vec![vec![0.0]]).unwrap();
2076
2077        assert!(MppiTerminalValueReplayBuffer2D::new(0).is_err());
2078        assert!(buffer.is_empty());
2079        assert!(matches!(
2080            buffer.update_grid(&mut grid, &updater),
2081            Err(RoboticsError::InvalidParameter(_))
2082        ));
2083    }
2084
2085    #[test]
2086    fn repeated_seed_is_reproducible() {
2087        let mut first = MppiController2D::new(small_config(11)).unwrap();
2088        let mut second = MppiController2D::new(small_config(11)).unwrap();
2089        let a = first
2090            .plan(MppiState2D::new(0.0, 0.0, 0.0, 0.0), (1.0, 0.0))
2091            .unwrap();
2092        let b = second
2093            .plan(MppiState2D::new(0.0, 0.0, 0.0, 0.0), (1.0, 0.0))
2094            .unwrap();
2095
2096        assert_eq!(a.first_control, b.first_control);
2097    }
2098
2099    #[test]
2100    fn rejects_invalid_config() {
2101        let config = MppiConfig {
2102            horizon: 0,
2103            ..MppiConfig::default()
2104        };
2105        assert!(matches!(
2106            MppiController2D::new(config),
2107            Err(RoboticsError::InvalidParameter(_))
2108        ));
2109    }
2110}