1use rand::{rngs::StdRng, SeedableRng};
7use rand_distr::{Distribution, Normal};
8use rust_robotics_core::{RoboticsError, RoboticsResult};
9use std::collections::VecDeque;
10
11#[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#[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#[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#[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#[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#[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#[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#[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#[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#[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#[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#[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
671pub 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#[derive(Debug, Clone, PartialEq)]
715pub struct MppiTerminalValueReplayBuffer2D {
716 capacity: usize,
717 rollouts: VecDeque<MppiRollout2D>,
718}
719
720#[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#[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#[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#[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#[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#[derive(Debug, Clone, PartialEq)]
894pub struct MppiSampledPlan2D {
895 pub plan: MppiPlan2D,
896 pub rollouts: Vec<MppiRollout2D>,
898 pub weights: Vec<f64>,
900}
901
902pub 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 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}