Skip to main content

rust_robotics_planning/
dwa.rs

1//! Dynamic Window Approach (DWA) local planner
2//!
3//! Implements velocity-space local planning for mobile robots.
4//! DWA searches the velocity space for admissible velocities and selects
5//! the best trajectory based on goal direction, speed, and obstacle clearance.
6
7use nalgebra::{Vector2, Vector4, Vector5};
8
9use rust_robotics_core::{
10    ControlInput, Obstacles, Path2D, Point2D, RoboticsError, RoboticsResult, State2D,
11};
12
13const ROBOT_STUCK_FLAG_CONS: f64 = 0.001;
14
15fn snap_to_resolution(value: f64, resolution: f64) -> f64 {
16    if !value.is_finite() || !resolution.is_finite() || resolution <= 0.0 {
17        return value;
18    }
19    (value / resolution).round() * resolution
20}
21
22fn arange_samples(start: f64, stop: f64, step: f64) -> Vec<f64> {
23    if !(start.is_finite() && stop.is_finite() && step.is_finite()) || step <= 0.0 || start >= stop
24    {
25        return Vec::new();
26    }
27    let count = ((stop - start) / step).ceil().max(0.0) as usize;
28    (0..count)
29        .map(|index| snap_to_resolution(start + step * index as f64, step))
30        .collect()
31}
32
33/// Robot state for DWA: [x, y, yaw, v, omega]
34pub type DWAState = Vector5<f64>;
35
36/// Control input: [v, omega]
37pub type DWAControl = Vector2<f64>;
38
39#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
40pub enum DWARobotType {
41    #[default]
42    Circle,
43    Rectangle,
44}
45
46fn rectangle_collision(
47    robot_x: f64,
48    robot_y: f64,
49    robot_yaw: f64,
50    obstacle_x: f64,
51    obstacle_y: f64,
52    robot_width: f64,
53    robot_length: f64,
54) -> bool {
55    let relative_x = obstacle_x - robot_x;
56    let relative_y = obstacle_y - robot_y;
57    let local_x = relative_x * robot_yaw.cos() + relative_y * robot_yaw.sin();
58    let local_y = -relative_x * robot_yaw.sin() + relative_y * robot_yaw.cos();
59    local_x <= robot_length / 2.0
60        && local_x >= -robot_length / 2.0
61        && local_y <= robot_width / 2.0
62        && local_y >= -robot_width / 2.0
63}
64
65/// Configuration for DWA planner
66#[derive(Debug, Clone)]
67pub struct DWAConfig {
68    pub max_speed: f64,
69    pub min_speed: f64,
70    pub max_yaw_rate: f64,
71    pub max_accel: f64,
72    pub max_delta_yaw_rate: f64,
73    pub v_resolution: f64,
74    pub yaw_rate_resolution: f64,
75    pub dt: f64,
76    pub predict_time: f64,
77    pub to_goal_cost_gain: f64,
78    pub speed_cost_gain: f64,
79    pub obstacle_cost_gain: f64,
80    pub robot_type: DWARobotType,
81    pub robot_radius: f64,
82    pub robot_width: f64,
83    pub robot_length: f64,
84    pub goal_threshold: f64,
85}
86
87impl Default for DWAConfig {
88    fn default() -> Self {
89        Self {
90            max_speed: 1.0,
91            min_speed: -0.5,
92            max_yaw_rate: 40.0_f64.to_radians(),
93            max_accel: 0.2,
94            max_delta_yaw_rate: 40.0_f64.to_radians(),
95            v_resolution: 0.01,
96            yaw_rate_resolution: 0.1_f64.to_radians(),
97            dt: 0.1,
98            predict_time: 3.0,
99            to_goal_cost_gain: 0.15,
100            speed_cost_gain: 1.0,
101            obstacle_cost_gain: 1.0,
102            robot_type: DWARobotType::Circle,
103            robot_radius: 1.0,
104            robot_width: 0.5,
105            robot_length: 1.2,
106            goal_threshold: 1.0,
107        }
108    }
109}
110
111impl DWAConfig {
112    pub fn validate(&self) -> RoboticsResult<()> {
113        if !self.max_speed.is_finite()
114            || !self.min_speed.is_finite()
115            || self.min_speed > self.max_speed
116        {
117            return Err(RoboticsError::InvalidParameter(
118                "DWA speed range must be finite and min_speed must be <= max_speed".to_string(),
119            ));
120        }
121        if !self.max_yaw_rate.is_finite() || self.max_yaw_rate <= 0.0 {
122            return Err(RoboticsError::InvalidParameter(
123                "DWA max_yaw_rate must be positive and finite".to_string(),
124            ));
125        }
126        if !self.max_accel.is_finite() || self.max_accel <= 0.0 {
127            return Err(RoboticsError::InvalidParameter(
128                "DWA max_accel must be positive and finite".to_string(),
129            ));
130        }
131        if !self.max_delta_yaw_rate.is_finite() || self.max_delta_yaw_rate <= 0.0 {
132            return Err(RoboticsError::InvalidParameter(
133                "DWA max_delta_yaw_rate must be positive and finite".to_string(),
134            ));
135        }
136        if !self.v_resolution.is_finite() || self.v_resolution <= 0.0 {
137            return Err(RoboticsError::InvalidParameter(
138                "DWA v_resolution must be positive and finite".to_string(),
139            ));
140        }
141        if !self.yaw_rate_resolution.is_finite() || self.yaw_rate_resolution <= 0.0 {
142            return Err(RoboticsError::InvalidParameter(
143                "DWA yaw_rate_resolution must be positive and finite".to_string(),
144            ));
145        }
146        if !self.dt.is_finite() || self.dt <= 0.0 {
147            return Err(RoboticsError::InvalidParameter(
148                "DWA dt must be positive and finite".to_string(),
149            ));
150        }
151        if !self.predict_time.is_finite() || self.predict_time <= 0.0 {
152            return Err(RoboticsError::InvalidParameter(
153                "DWA predict_time must be positive and finite".to_string(),
154            ));
155        }
156        if !self.to_goal_cost_gain.is_finite() || self.to_goal_cost_gain < 0.0 {
157            return Err(RoboticsError::InvalidParameter(
158                "DWA to_goal_cost_gain must be non-negative and finite".to_string(),
159            ));
160        }
161        if !self.speed_cost_gain.is_finite() || self.speed_cost_gain < 0.0 {
162            return Err(RoboticsError::InvalidParameter(
163                "DWA speed_cost_gain must be non-negative and finite".to_string(),
164            ));
165        }
166        if !self.obstacle_cost_gain.is_finite() || self.obstacle_cost_gain < 0.0 {
167            return Err(RoboticsError::InvalidParameter(
168                "DWA obstacle_cost_gain must be non-negative and finite".to_string(),
169            ));
170        }
171        if !self.robot_width.is_finite() || self.robot_width < 0.0 {
172            return Err(RoboticsError::InvalidParameter(
173                "DWA robot_width must be non-negative and finite".to_string(),
174            ));
175        }
176        if !self.robot_length.is_finite() || self.robot_length < 0.0 {
177            return Err(RoboticsError::InvalidParameter(
178                "DWA robot_length must be non-negative and finite".to_string(),
179            ));
180        }
181        if self.robot_type == DWARobotType::Rectangle
182            && (self.robot_width <= 0.0 || self.robot_length <= 0.0)
183        {
184            return Err(RoboticsError::InvalidParameter(
185                "DWA rectangle robot dimensions must be positive".to_string(),
186            ));
187        }
188        if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
189            return Err(RoboticsError::InvalidParameter(
190                "DWA robot_radius must be non-negative and finite".to_string(),
191            ));
192        }
193        if !self.goal_threshold.is_finite() || self.goal_threshold < 0.0 {
194            return Err(RoboticsError::InvalidParameter(
195                "DWA goal_threshold must be non-negative and finite".to_string(),
196            ));
197        }
198        Ok(())
199    }
200}
201
202/// Dynamic window bounds: [v_min, v_max, yaw_rate_min, yaw_rate_max]
203pub type DynamicWindow = Vector4<f64>;
204
205/// One actually sampled constant-velocity arc, including rejected collisions.
206#[derive(Debug, Clone)]
207pub struct DWACandidate {
208    pub trajectory: Trajectory,
209    /// Weighted heading and speed costs, also available for rejected arcs.
210    pub goal_cost: f64,
211    pub speed_cost: f64,
212    /// `None` means the sampled robot footprint intersects an obstacle.
213    pub obstacle_cost: Option<f64>,
214}
215
216/// Diagnostics of one decision, in the planner's actual sampling order.
217///
218/// The stuck-robot rule can override the selected arc's angular command.
219/// `applied_control` exposes that distinction; the sampled trajectory is not
220/// rewritten to imply that it predicted the overridden command.
221#[derive(Debug, Clone)]
222pub struct DWAPlanTrace {
223    pub dynamic_window: DynamicWindow,
224    pub candidates: Vec<DWACandidate>,
225    pub selected: Option<usize>,
226    pub applied_control: DWAControl,
227    pub stuck_override: bool,
228}
229
230/// Predicted trajectory
231#[derive(Debug, Clone)]
232pub struct Trajectory {
233    pub states: Vec<DWAState>,
234    pub control: DWAControl,
235    pub cost: f64,
236}
237
238impl Trajectory {
239    pub fn new() -> Self {
240        Self {
241            states: Vec::new(),
242            control: DWAControl::zeros(),
243            cost: f64::MAX,
244        }
245    }
246
247    pub fn final_state(&self) -> Option<&DWAState> {
248        self.states.last()
249    }
250
251    pub fn to_path(&self) -> Path2D {
252        let points: Vec<Point2D> = self
253            .states
254            .iter()
255            .map(|s| Point2D::new(s[0], s[1]))
256            .collect();
257        Path2D::from_points(points)
258    }
259}
260
261impl Default for Trajectory {
262    fn default() -> Self {
263        Self::new()
264    }
265}
266
267/// Dynamic Window Approach local planner
268pub struct DWAPlanner {
269    config: DWAConfig,
270    state: DWAState,
271    goal: Point2D,
272    obstacles: Vec<Point2D>,
273    best_trajectory: Trajectory,
274}
275
276impl DWAPlanner {
277    pub fn new(config: DWAConfig) -> Self {
278        Self::try_new(config).expect("invalid DWA configuration")
279    }
280
281    pub fn try_new(config: DWAConfig) -> RoboticsResult<Self> {
282        config.validate()?;
283        Ok(Self {
284            config,
285            state: DWAState::zeros(),
286            goal: Point2D::origin(),
287            obstacles: Vec::new(),
288            best_trajectory: Trajectory::new(),
289        })
290    }
291
292    pub fn with_defaults() -> Self {
293        Self::new(DWAConfig::default())
294    }
295
296    pub fn set_state(&mut self, state: DWAState) {
297        self.try_set_state(state)
298            .expect("DWA state must contain only finite values")
299    }
300
301    pub fn try_set_state(&mut self, state: DWAState) -> RoboticsResult<()> {
302        Self::validate_state(&state)?;
303        self.state = state;
304        Ok(())
305    }
306
307    pub fn set_state_from_2d(&mut self, state: &State2D, omega: f64) {
308        self.try_set_state_from_2d(state, omega)
309            .expect("DWA state must contain only finite values")
310    }
311
312    pub fn try_set_state_from_2d(&mut self, state: &State2D, omega: f64) -> RoboticsResult<()> {
313        self.try_set_state(DWAState::new(state.x, state.y, state.yaw, state.v, omega))
314    }
315
316    pub fn get_state(&self) -> &DWAState {
317        &self.state
318    }
319
320    pub fn state_2d(&self) -> State2D {
321        State2D::new(self.state[0], self.state[1], self.state[2], self.state[3])
322    }
323
324    pub fn set_goal(&mut self, goal: Point2D) {
325        self.try_set_goal(goal)
326            .expect("DWA goal must contain only finite values")
327    }
328
329    pub fn try_set_goal(&mut self, goal: Point2D) -> RoboticsResult<()> {
330        Self::validate_goal(&goal)?;
331        self.goal = goal;
332        Ok(())
333    }
334
335    pub fn set_obstacles(&mut self, obstacles: Vec<Point2D>) {
336        self.try_set_obstacles(obstacles)
337            .expect("DWA obstacles must contain only finite values")
338    }
339
340    pub fn try_set_obstacles(&mut self, obstacles: Vec<Point2D>) -> RoboticsResult<()> {
341        Self::validate_obstacles(&obstacles)?;
342        self.obstacles = obstacles;
343        Ok(())
344    }
345
346    pub fn set_obstacles_from_tuples(&mut self, obstacles: &[(f64, f64)]) {
347        self.try_set_obstacles(
348            obstacles
349                .iter()
350                .map(|(x, y)| Point2D::new(*x, *y))
351                .collect(),
352        )
353        .expect("DWA obstacles must contain only finite values")
354    }
355
356    pub fn set_obstacles_from_obstacles(&mut self, obstacles: &Obstacles) -> RoboticsResult<()> {
357        self.try_set_obstacles(obstacles.points.clone())
358    }
359
360    pub fn get_best_trajectory(&self) -> &Trajectory {
361        &self.best_trajectory
362    }
363    pub fn best_path(&self) -> Path2D {
364        self.best_trajectory.to_path()
365    }
366    pub fn config(&self) -> &DWAConfig {
367        &self.config
368    }
369
370    fn motion(&self, state: &DWAState, control: &DWAControl) -> DWAState {
371        let dt = self.config.dt;
372        let mut next = *state;
373        next[2] += control[1] * dt;
374        next[0] += control[0] * next[2].cos() * dt;
375        next[1] += control[0] * next[2].sin() * dt;
376        next[3] = control[0];
377        next[4] = control[1];
378        next
379    }
380
381    fn calc_dynamic_window(&self) -> DynamicWindow {
382        let config = &self.config;
383        let state = &self.state;
384        let vs = Vector4::new(
385            config.min_speed,
386            config.max_speed,
387            -config.max_yaw_rate,
388            config.max_yaw_rate,
389        );
390        let vd = Vector4::new(
391            state[3] - config.max_accel * config.dt,
392            state[3] + config.max_accel * config.dt,
393            state[4] - config.max_delta_yaw_rate * config.dt,
394            state[4] + config.max_delta_yaw_rate * config.dt,
395        );
396        DynamicWindow::new(
397            vs[0].max(vd[0]),
398            vs[1].min(vd[1]),
399            vs[2].max(vd[2]),
400            vs[3].min(vd[3]),
401        )
402    }
403
404    fn predict_trajectory(&self, v: f64, omega: f64) -> Trajectory {
405        let config = &self.config;
406        let mut state = self.state;
407        let control = DWAControl::new(v, omega);
408        let mut states = Vec::new();
409        states.push(state);
410        let mut time = 0.0;
411        while time <= config.predict_time {
412            state = self.motion(&state, &control);
413            states.push(state);
414            time += config.dt;
415        }
416        Trajectory {
417            states,
418            control,
419            cost: 0.0,
420        }
421    }
422
423    fn calc_to_goal_cost(&self, trajectory: &Trajectory) -> f64 {
424        if let Some(final_state) = trajectory.final_state() {
425            let dx = self.goal.x - final_state[0];
426            let dy = self.goal.y - final_state[1];
427            let target_angle = dy.atan2(dx);
428            let heading_error = target_angle - final_state[2];
429            heading_error.sin().atan2(heading_error.cos()).abs()
430        } else {
431            f64::MAX
432        }
433    }
434
435    fn calc_speed_cost(&self, trajectory: &Trajectory) -> f64 {
436        if let Some(final_state) = trajectory.final_state() {
437            self.config.max_speed - final_state[3]
438        } else {
439            f64::MAX
440        }
441    }
442
443    fn calc_obstacle_cost(&self, trajectory: &Trajectory) -> f64 {
444        if self.obstacles.is_empty() {
445            return 0.0;
446        }
447        let mut min_dist = f64::MAX;
448        for state in &trajectory.states {
449            for obs in &self.obstacles {
450                let dx = state[0] - obs.x;
451                let dy = state[1] - obs.y;
452                let dist = (dx * dx + dy * dy).sqrt();
453                let collision = match self.config.robot_type {
454                    DWARobotType::Circle => dist <= self.config.robot_radius,
455                    DWARobotType::Rectangle => rectangle_collision(
456                        state[0],
457                        state[1],
458                        state[2],
459                        obs.x,
460                        obs.y,
461                        self.config.robot_width,
462                        self.config.robot_length,
463                    ),
464                };
465                if collision {
466                    return f64::MAX;
467                }
468                if dist < min_dist {
469                    min_dist = dist;
470                }
471            }
472        }
473        if min_dist < f64::MAX {
474            1.0 / min_dist
475        } else {
476            0.0
477        }
478    }
479
480    pub fn plan_step(&mut self) -> DWAControl {
481        self.try_plan_step().expect("invalid DWA planning state")
482    }
483
484    pub fn try_plan_step(&mut self) -> RoboticsResult<DWAControl> {
485        self.plan_step_internal(None)
486    }
487
488    /// Plan with the same costs, tie break, collision checks and stuck rule as
489    /// `try_plan_step`, retaining every sampled arc for inspection.
490    pub fn try_plan_step_with_trace(&mut self) -> RoboticsResult<DWAPlanTrace> {
491        let mut trace = DWAPlanTrace {
492            dynamic_window: self.calc_dynamic_window(),
493            candidates: Vec::new(),
494            selected: None,
495            applied_control: DWAControl::zeros(),
496            stuck_override: false,
497        };
498        self.plan_step_internal(Some(&mut trace))?;
499        Ok(trace)
500    }
501
502    fn plan_step_internal(
503        &mut self,
504        mut trace: Option<&mut DWAPlanTrace>,
505    ) -> RoboticsResult<DWAControl> {
506        self.config.validate()?;
507        Self::validate_state(&self.state)?;
508        Self::validate_goal(&self.goal)?;
509        Self::validate_obstacles(&self.obstacles)?;
510
511        let config = &self.config;
512        let dw = self.calc_dynamic_window();
513        let mut best_trajectory = Trajectory {
514            states: vec![self.state],
515            control: DWAControl::zeros(),
516            cost: f64::MAX,
517        };
518        let mut min_cost = f64::MAX;
519
520        for v in arange_samples(dw[0], dw[1], config.v_resolution) {
521            for omega in arange_samples(dw[2], dw[3], config.yaw_rate_resolution) {
522                let mut trajectory = self.predict_trajectory(v, omega);
523                // A colliding arc is never a candidate, whatever the gain
524                // (f64::MAX times a gain below 1 used to rank below other
525                // costs, and an all-colliding window returned an arc into
526                // the obstacle). With every arc colliding the result is a
527                // stop, with cost f64::MAX.
528                let raw_obstacle_cost = self.calc_obstacle_cost(&trajectory);
529                if raw_obstacle_cost == f64::MAX {
530                    if let Some(t) = trace.as_deref_mut() {
531                        let goal_cost =
532                            config.to_goal_cost_gain * self.calc_to_goal_cost(&trajectory);
533                        let speed_cost = config.speed_cost_gain * self.calc_speed_cost(&trajectory);
534                        trajectory.cost = f64::MAX;
535                        t.candidates.push(DWACandidate {
536                            trajectory,
537                            goal_cost,
538                            speed_cost,
539                            obstacle_cost: None,
540                        });
541                    }
542                    continue;
543                }
544                let goal_cost = config.to_goal_cost_gain * self.calc_to_goal_cost(&trajectory);
545                let speed_cost = config.speed_cost_gain * self.calc_speed_cost(&trajectory);
546                let obstacle_cost = config.obstacle_cost_gain * raw_obstacle_cost;
547                let total_cost = goal_cost + speed_cost + obstacle_cost;
548                if let Some(t) = trace.as_deref_mut() {
549                    trajectory.cost = total_cost;
550                    t.candidates.push(DWACandidate {
551                        trajectory: trajectory.clone(),
552                        goal_cost,
553                        speed_cost,
554                        obstacle_cost: Some(obstacle_cost),
555                    });
556                }
557                if total_cost <= min_cost {
558                    min_cost = total_cost;
559                    let mut control = DWAControl::new(v, omega);
560                    if control[0].abs() < ROBOT_STUCK_FLAG_CONS
561                        && self.state[3].abs() < ROBOT_STUCK_FLAG_CONS
562                    {
563                        control[1] = -config.max_delta_yaw_rate;
564                    }
565                    if let Some(t) = trace.as_deref_mut() {
566                        t.selected = Some(t.candidates.len() - 1);
567                        t.stuck_override = control[1] != omega;
568                    }
569                    best_trajectory = Trajectory {
570                        states: trajectory.states,
571                        control,
572                        cost: total_cost,
573                    };
574                }
575            }
576        }
577
578        self.best_trajectory = best_trajectory;
579        if let Some(t) = trace {
580            t.applied_control = self.best_trajectory.control;
581        }
582        Ok(self.best_trajectory.control)
583    }
584
585    pub fn step(&mut self) -> DWAControl {
586        self.try_step().expect("invalid DWA step")
587    }
588
589    pub fn try_step(&mut self) -> RoboticsResult<DWAControl> {
590        let control = self.try_plan_step()?;
591        self.state = self.motion(&self.state, &control);
592        Ok(control)
593    }
594
595    pub fn is_goal_reached(&self) -> bool {
596        let dx = self.state[0] - self.goal.x;
597        let dy = self.state[1] - self.goal.y;
598        (dx * dx + dy * dy).sqrt() <= self.config.goal_threshold
599    }
600
601    pub fn distance_to_goal(&self) -> f64 {
602        let dx = self.state[0] - self.goal.x;
603        let dy = self.state[1] - self.goal.y;
604        (dx * dx + dy * dy).sqrt()
605    }
606
607    pub fn navigate_to_goal(&mut self, max_steps: usize) -> Vec<DWAState> {
608        self.try_navigate_to_goal(max_steps)
609            .expect("invalid DWA navigation")
610    }
611
612    pub fn try_navigate_to_goal(&mut self, max_steps: usize) -> RoboticsResult<Vec<DWAState>> {
613        let mut path = vec![self.state];
614        for _ in 0..max_steps {
615            if self.is_goal_reached() {
616                break;
617            }
618            self.try_step()?;
619            path.push(self.state);
620        }
621        Ok(path)
622    }
623
624    pub fn try_plan_input(&mut self) -> RoboticsResult<ControlInput> {
625        let control = self.try_plan_step()?;
626        Ok(Self::control_to_input(&control))
627    }
628
629    pub fn control_to_input(control: &DWAControl) -> ControlInput {
630        ControlInput::new(control[0], control[1])
631    }
632
633    fn validate_state(state: &DWAState) -> RoboticsResult<()> {
634        if state.iter().any(|value| !value.is_finite()) {
635            return Err(RoboticsError::InvalidParameter(
636                "DWA state must contain only finite values".to_string(),
637            ));
638        }
639        Ok(())
640    }
641
642    fn validate_goal(goal: &Point2D) -> RoboticsResult<()> {
643        if !goal.x.is_finite() || !goal.y.is_finite() {
644            return Err(RoboticsError::InvalidParameter(
645                "DWA goal must contain only finite values".to_string(),
646            ));
647        }
648        Ok(())
649    }
650
651    fn validate_obstacles(obstacles: &[Point2D]) -> RoboticsResult<()> {
652        if obstacles
653            .iter()
654            .any(|o| !o.x.is_finite() || !o.y.is_finite())
655        {
656            return Err(RoboticsError::InvalidParameter(
657                "DWA obstacles must contain only finite values".to_string(),
658            ));
659        }
660        Ok(())
661    }
662}
663
664// Legacy interface
665#[derive(Debug, Clone)]
666#[deprecated(note = "Use DWAConfig instead")]
667pub struct Config {
668    pub max_speed: f64,
669    pub min_speed: f64,
670    pub max_yaw_rate: f64,
671    pub max_accel: f64,
672    pub max_delta_yaw_rate: f64,
673    pub v_resolution: f64,
674    pub yaw_rate_resolution: f64,
675    pub dt: f64,
676    pub predict_time: f64,
677    pub to_goal_cost_gain: f64,
678    pub speed_cost_gain: f64,
679    pub obstacle_cost_gain: f64,
680    pub robot_type: DWARobotType,
681    pub robot_radius: f64,
682    pub robot_width: f64,
683    pub robot_length: f64,
684}
685
686pub fn motion(mut x: Vector5<f64>, u: Vector2<f64>, dt: f64) -> Vector5<f64> {
687    x[2] += u[1] * dt;
688    x[0] += u[0] * x[2].cos() * dt;
689    x[1] += u[0] * x[2].sin() * dt;
690    x[3] = u[0];
691    x[4] = u[1];
692    x
693}
694
695#[allow(deprecated)]
696pub fn calc_dynamic_window(x: Vector5<f64>, config: &Config) -> Vector4<f64> {
697    let vs = Vector4::new(
698        config.min_speed,
699        config.max_speed,
700        -config.max_yaw_rate,
701        config.max_yaw_rate,
702    );
703    let vd = Vector4::new(
704        x[3] - config.max_accel * config.dt,
705        x[3] + config.max_accel * config.dt,
706        x[4] - config.max_delta_yaw_rate * config.dt,
707        x[4] + config.max_delta_yaw_rate * config.dt,
708    );
709    Vector4::new(
710        vs[0].max(vd[0]),
711        vs[1].min(vd[1]),
712        vs[2].max(vd[2]),
713        vs[3].min(vd[3]),
714    )
715}
716
717#[allow(deprecated)]
718pub fn predict_trajectory(
719    x_init: Vector5<f64>,
720    v: f64,
721    y: f64,
722    config: &Config,
723) -> Vec<Vector5<f64>> {
724    let mut x = x_init;
725    let mut trajectory = vec![x_init];
726    let mut time = 0.0;
727    while time <= config.predict_time {
728        x = motion(x, Vector2::new(v, y), config.dt);
729        trajectory.push(x);
730        time += config.dt;
731    }
732    trajectory
733}
734
735#[allow(deprecated)]
736pub fn calc_obstacle_cost(trajectory: &[Vector5<f64>], ob: &[(f64, f64)], config: &Config) -> f64 {
737    if trajectory.is_empty() || ob.is_empty() {
738        return 0.0;
739    }
740    let mut min_r = f64::MAX;
741    for state in trajectory {
742        for obstacle in ob {
743            let r = ((state[0] - obstacle.0).powi(2) + (state[1] - obstacle.1).powi(2)).sqrt();
744            let collision = match config.robot_type {
745                DWARobotType::Circle => r <= config.robot_radius,
746                DWARobotType::Rectangle => rectangle_collision(
747                    state[0],
748                    state[1],
749                    state[2],
750                    obstacle.0,
751                    obstacle.1,
752                    config.robot_width,
753                    config.robot_length,
754                ),
755            };
756            if collision {
757                return f64::MAX;
758            }
759            if r < min_r {
760                min_r = r;
761            }
762        }
763    }
764    if min_r < f64::MAX {
765        1.0 / min_r
766    } else {
767        0.0
768    }
769}
770
771pub fn calc_to_goal_cost(trajectory: &[Vector5<f64>], goal: (f64, f64)) -> f64 {
772    if trajectory.is_empty() {
773        return f64::MAX;
774    }
775    let last = &trajectory[trajectory.len() - 1];
776    let dx = goal.0 - last[0];
777    let dy = goal.1 - last[1];
778    let error_angle = dy.atan2(dx);
779    let cost_angle = error_angle - last[2];
780    cost_angle.sin().atan2(cost_angle.cos()).abs()
781}
782
783#[allow(deprecated)]
784pub fn dwa_control(
785    x: Vector5<f64>,
786    config: &Config,
787    goal: (f64, f64),
788    ob: &[(f64, f64)],
789) -> ((f64, f64), Vec<Vector5<f64>>) {
790    let dw = calc_dynamic_window(x, config);
791    calc_control_and_trajectory(x, dw, config, goal, ob)
792}
793
794#[allow(deprecated)]
795pub fn calc_control_and_trajectory(
796    x: Vector5<f64>,
797    dw: Vector4<f64>,
798    config: &Config,
799    goal: (f64, f64),
800    ob: &[(f64, f64)],
801) -> ((f64, f64), Vec<Vector5<f64>>) {
802    let mut min_cost = f64::MAX;
803    let mut best_u = (0.0, 0.0);
804    let mut best_trajectory = vec![x];
805    for v in arange_samples(dw[0], dw[1], config.v_resolution) {
806        for y in arange_samples(dw[2], dw[3], config.yaw_rate_resolution) {
807            let trajectory = predict_trajectory(x, v, y, config);
808            let to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(&trajectory, goal);
809            let speed_cost =
810                config.speed_cost_gain * (config.max_speed - trajectory.last().unwrap()[3]);
811            let ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(&trajectory, ob, config);
812            let final_cost = to_goal_cost + speed_cost + ob_cost;
813            if final_cost <= min_cost {
814                min_cost = final_cost;
815                best_u = (v, y);
816                best_trajectory = trajectory;
817                if best_u.0.abs() < ROBOT_STUCK_FLAG_CONS && x[3].abs() < ROBOT_STUCK_FLAG_CONS {
818                    best_u.1 = -config.max_delta_yaw_rate;
819                }
820            }
821        }
822    }
823    (best_u, best_trajectory)
824}
825
826#[cfg(test)]
827mod tests {
828    use super::*;
829
830    #[derive(Clone, Copy)]
831    struct ClosedLoopSample {
832        control: [f64; 2],
833        state: [f64; 5],
834        predicted_len: usize,
835        predicted_last: [f64; 5],
836        dist: f64,
837    }
838
839    fn assert_close(actual: f64, expected: f64) {
840        assert!(
841            (actual - expected).abs() < 1.0e-12,
842            "expected {expected}, got {actual}"
843        );
844    }
845
846    fn assert_close_tol(actual: f64, expected: f64, tol: f64) {
847        assert!(
848            (actual - expected).abs() < tol,
849            "expected {expected}, got {actual} (tol={tol})"
850        );
851    }
852
853    fn assert_state_close(actual: &DWAState, expected: &[f64; 5]) {
854        for (value, expected) in actual.iter().zip(expected.iter()) {
855            assert_close(*value, *expected);
856        }
857    }
858
859    fn assert_state_close_tol(actual: &[f64; 5], expected: &[f64; 5], tol: f64) {
860        for (value, expected) in actual.iter().zip(expected.iter()) {
861            assert_close_tol(*value, *expected, tol);
862        }
863    }
864
865    fn assert_sample_close(actual: &ClosedLoopSample, expected: &ClosedLoopSample) {
866        assert_close_tol(actual.control[0], expected.control[0], 0.05);
867        assert_close_tol(actual.control[1], expected.control[1], 0.05);
868        assert_state_close_tol(&actual.state, &expected.state, 0.25);
869        assert_eq!(actual.predicted_len, expected.predicted_len);
870        assert_state_close_tol(&actual.predicted_last, &expected.predicted_last, 0.25);
871        assert_close_tol(actual.dist, expected.dist, 0.2);
872    }
873
874    fn to_state_array(state: &DWAState) -> [f64; 5] {
875        [state[0], state[1], state[2], state[3], state[4]]
876    }
877
878    fn run_closed_loop_trace(
879        mut dwa: DWAPlanner,
880        max_steps: usize,
881    ) -> RoboticsResult<Vec<ClosedLoopSample>> {
882        let mut trace = Vec::new();
883        for _step in 0..max_steps {
884            let control = dwa.try_plan_step()?;
885            let predicted = dwa.get_best_trajectory();
886            let predicted_len = predicted.states.len();
887            let predicted_last = to_state_array(predicted.final_state().unwrap());
888            let next_state = dwa.motion(&dwa.state, &control);
889            dwa.state = next_state;
890            trace.push(ClosedLoopSample {
891                control: [control[0], control[1]],
892                state: to_state_array(&dwa.state),
893                predicted_len,
894                predicted_last,
895                dist: dwa.distance_to_goal(),
896            });
897            if dwa.is_goal_reached() {
898                break;
899            }
900        }
901        Ok(trace)
902    }
903
904    fn pythonrobotics_default_obstacles() -> Vec<Point2D> {
905        vec![
906            Point2D::new(-1.0, -1.0),
907            Point2D::new(0.0, 2.0),
908            Point2D::new(4.0, 2.0),
909            Point2D::new(5.0, 4.0),
910            Point2D::new(5.0, 5.0),
911            Point2D::new(5.0, 6.0),
912            Point2D::new(5.0, 9.0),
913            Point2D::new(8.0, 9.0),
914            Point2D::new(7.0, 9.0),
915            Point2D::new(8.0, 10.0),
916            Point2D::new(9.0, 11.0),
917            Point2D::new(12.0, 13.0),
918            Point2D::new(12.0, 12.0),
919            Point2D::new(15.0, 15.0),
920            Point2D::new(13.0, 13.0),
921        ]
922    }
923
924    fn pythonrobotics_stuck_obstacles() -> Vec<Point2D> {
925        vec![
926            Point2D::new(1.0, 1.0),
927            Point2D::new(-0.0, -2.0),
928            Point2D::new(-2.0, -6.0),
929            Point2D::new(-2.0, -8.0),
930            Point2D::new(-3.0, -9.27),
931            Point2D::new(-3.79, -9.39),
932            Point2D::new(-7.25, -8.97),
933            Point2D::new(-7.0, -2.0),
934            Point2D::new(-3.0, -4.0),
935            Point2D::new(-6.0, -5.0),
936            Point2D::new(-3.5, -5.8),
937            Point2D::new(-6.0, -9.0),
938            Point2D::new(-8.8, -9.0),
939            Point2D::new(-5.0, -9.0),
940            Point2D::new(-7.5, -3.0),
941            Point2D::new(-9.0, -8.0),
942            Point2D::new(-5.8, -4.4),
943            Point2D::new(-12.0, -12.0),
944            Point2D::new(-3.0, -2.0),
945            Point2D::new(-13.0, -13.0),
946        ]
947    }
948
949    #[test]
950    fn trace_preserves_decisions_including_ties_rejections_and_stuck_override() {
951        for config in [
952            DWAConfig::default(),
953            DWAConfig {
954                obstacle_cost_gain: 0.0,
955                ..DWAConfig::default()
956            },
957        ] {
958            for blocked in [false, true] {
959                for speed in [0.0, 0.6] {
960                    let mut a = DWAPlanner::new(config.clone());
961                    let mut b = DWAPlanner::new(config.clone());
962                    let state = DWAState::new(0., 0., 0., speed, 0.);
963                    for p in [&mut a, &mut b] {
964                        p.set_state(state);
965                        p.set_goal(Point2D::new(5., 0.));
966                        if blocked {
967                            p.set_obstacles(vec![Point2D::origin()]);
968                        }
969                    }
970                    let expected = a.plan_step();
971                    let trace = b.try_plan_step_with_trace().unwrap();
972                    assert_eq!(expected, trace.applied_control);
973                    assert_eq!(
974                        a.get_best_trajectory().states,
975                        b.get_best_trajectory().states
976                    );
977                    assert_eq!(a.get_best_trajectory().cost, b.get_best_trajectory().cost);
978                    assert!(!trace.candidates.is_empty());
979                    if blocked {
980                        assert!(trace.selected.is_none());
981                        assert!(trace.candidates.iter().all(|c| c.obstacle_cost.is_none()));
982                        assert_eq!(expected, DWAControl::zeros());
983                    } else {
984                        let i = trace.selected.unwrap();
985                        let cost = trace.candidates[i].trajectory.cost;
986                        assert!(trace.candidates.iter().all(|c| c.trajectory.cost >= cost));
987                        assert_eq!(
988                            trace
989                                .candidates
990                                .iter()
991                                .rposition(|c| c.trajectory.cost == cost),
992                            Some(i)
993                        );
994                    }
995                }
996            }
997        }
998        let mut p = DWAPlanner::new(DWAConfig {
999            min_speed: 0.,
1000            max_speed: 0.01,
1001            v_resolution: 1.,
1002            ..DWAConfig::default()
1003        });
1004        p.set_goal(Point2D::new(5., 0.));
1005        let t = p.try_plan_step_with_trace().unwrap();
1006        assert!(t.stuck_override);
1007        assert_eq!(t.applied_control[1], -p.config().max_delta_yaw_rate);
1008        assert_ne!(
1009            t.applied_control,
1010            t.candidates[t.selected.unwrap()].trajectory.control
1011        );
1012        // The legacy stuck rule can put yaw velocity outside the physical
1013        // cap. The following window is truly empty, not a reversed interval.
1014        let mut p = DWAPlanner::new(DWAConfig {
1015            min_speed: 0.,
1016            max_speed: 0.01,
1017            v_resolution: 1.,
1018            max_yaw_rate: 1.2,
1019            max_delta_yaw_rate: 2.,
1020            dt: 0.2,
1021            ..DWAConfig::default()
1022        });
1023        p.set_goal(Point2D::new(5., 0.));
1024        let first = p.try_plan_step_with_trace().unwrap();
1025        assert!(first.stuck_override);
1026        p.set_state(motion(*p.get_state(), first.applied_control, 0.2));
1027        let next = p.try_plan_step_with_trace().unwrap();
1028        assert!(next.dynamic_window[2] > next.dynamic_window[3]);
1029        assert!(next.candidates.is_empty());
1030        assert!(next.selected.is_none());
1031        assert_eq!(next.applied_control, DWAControl::zeros());
1032        assert_eq!(p.plan_step(), next.applied_control);
1033    }
1034
1035    #[test]
1036    fn test_dwa_creation() {
1037        let dwa = DWAPlanner::with_defaults();
1038        assert_eq!(dwa.state, DWAState::zeros());
1039    }
1040
1041    #[test]
1042    fn test_dwa_config_default() {
1043        let config = DWAConfig::default();
1044        assert_eq!(config.max_speed, 1.0);
1045        assert_eq!(config.robot_type, DWARobotType::Circle);
1046        assert_eq!(config.robot_radius, 1.0);
1047        assert_eq!(config.robot_width, 0.5);
1048        assert_eq!(config.robot_length, 1.2);
1049    }
1050
1051    #[test]
1052    fn test_dwa_set_goal() {
1053        let mut dwa = DWAPlanner::with_defaults();
1054        dwa.set_goal(Point2D::new(10.0, 10.0));
1055        assert_eq!(dwa.goal.x, 10.0);
1056        assert_eq!(dwa.goal.y, 10.0);
1057    }
1058
1059    #[test]
1060    fn test_dwa_motion() {
1061        let dwa = DWAPlanner::with_defaults();
1062        let state = DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0);
1063        let control = DWAControl::new(1.0, 0.0);
1064        let next = dwa.motion(&state, &control);
1065        assert!(next[0] > 0.0);
1066        assert_eq!(next[3], 1.0);
1067    }
1068
1069    #[test]
1070    fn test_dwa_dynamic_window() {
1071        let mut dwa = DWAPlanner::with_defaults();
1072        dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.5, 0.0));
1073        let dw = dwa.calc_dynamic_window();
1074        assert!(dw[0] >= dwa.config.min_speed);
1075        assert!(dw[1] <= dwa.config.max_speed);
1076    }
1077
1078    #[test]
1079    fn test_dwa_predict_trajectory() {
1080        let dwa = DWAPlanner::with_defaults();
1081        let trajectory = dwa.predict_trajectory(1.0, 0.0);
1082        assert!(!trajectory.states.is_empty());
1083        let final_state = trajectory.final_state().unwrap();
1084        assert!(final_state[0] > 0.0);
1085    }
1086
1087    #[test]
1088    fn test_dwa_obstacle_cost() {
1089        let mut dwa = DWAPlanner::with_defaults();
1090        dwa.set_obstacles(vec![Point2D::new(100.0, 100.0)]);
1091        let trajectory = dwa.predict_trajectory(1.0, 0.0);
1092        let cost = dwa.calc_obstacle_cost(&trajectory);
1093        assert!(cost < 1.0);
1094    }
1095
1096    #[test]
1097    fn test_dwa_collision_detection() {
1098        let mut dwa = DWAPlanner::with_defaults();
1099        dwa.set_obstacles(vec![Point2D::new(0.5, 0.0)]);
1100        let trajectory = dwa.predict_trajectory(1.0, 0.0);
1101        let cost = dwa.calc_obstacle_cost(&trajectory);
1102        assert_eq!(cost, f64::MAX);
1103    }
1104
1105    #[test]
1106    fn test_dwa_plan_step() {
1107        let mut dwa = DWAPlanner::with_defaults();
1108        dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0));
1109        dwa.set_goal(Point2D::new(10.0, 0.0));
1110        let control = dwa.plan_step();
1111        assert!(control[0] > 0.0);
1112    }
1113
1114    #[test]
1115    fn test_dwa_goal_reached() {
1116        let mut dwa = DWAPlanner::with_defaults();
1117        dwa.set_state(DWAState::new(10.0, 10.0, 0.0, 0.0, 0.0));
1118        dwa.set_goal(Point2D::new(10.0, 10.0));
1119        assert!(dwa.is_goal_reached());
1120    }
1121
1122    #[test]
1123    fn test_dwa_navigate() {
1124        let mut dwa = DWAPlanner::with_defaults();
1125        dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0));
1126        dwa.set_goal(Point2D::new(3.0, 0.0));
1127        let path = dwa.navigate_to_goal(100);
1128        assert!(path.len() > 1);
1129        let final_state = path.last().unwrap();
1130        let dist = ((final_state[0] - 3.0).powi(2) + final_state[1].powi(2)).sqrt();
1131        assert!(dist <= dwa.config.goal_threshold);
1132    }
1133
1134    #[test]
1135    fn test_trajectory_to_path() {
1136        let dwa = DWAPlanner::with_defaults();
1137        let trajectory = dwa.predict_trajectory(1.0, 0.1);
1138        let path = trajectory.to_path();
1139        assert!(!path.is_empty());
1140    }
1141
1142    #[test]
1143    fn test_legacy_motion() {
1144        let x = Vector5::new(0.0, 0.0, 0.0, 0.0, 0.0);
1145        let u = Vector2::new(1.0, 0.0);
1146        let next = motion(x, u, 0.1);
1147        assert!(next[0] > 0.0);
1148    }
1149
1150    #[test]
1151    fn test_dwa_try_new_rejects_invalid_config() {
1152        let config = DWAConfig {
1153            dt: 0.0,
1154            ..Default::default()
1155        };
1156        let err = match DWAPlanner::try_new(config) {
1157            Ok(_) => panic!("expected invalid config to be rejected"),
1158            Err(err) => err,
1159        };
1160        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
1161    }
1162
1163    #[test]
1164    fn test_dwa_state_2d_matches_internal_state() {
1165        let mut dwa = DWAPlanner::with_defaults();
1166        dwa.set_state(DWAState::new(1.0, 2.0, 0.3, 0.4, 0.1));
1167        let state = dwa.state_2d();
1168        assert_eq!(state.x, 1.0);
1169        assert_eq!(state.y, 2.0);
1170        assert_eq!(state.yaw, 0.3);
1171        assert_eq!(state.v, 0.4);
1172    }
1173
1174    #[test]
1175    fn test_dwa_set_obstacles_from_obstacles() {
1176        let mut dwa = DWAPlanner::with_defaults();
1177        let obstacles =
1178            Obstacles::from_points(vec![Point2D::new(1.0, 1.0), Point2D::new(2.0, 2.0)]);
1179        dwa.set_obstacles_from_obstacles(&obstacles).unwrap();
1180        assert_eq!(dwa.obstacles.len(), 2);
1181    }
1182
1183    #[test]
1184    fn test_dwa_try_plan_input_returns_common_control() {
1185        let mut dwa = DWAPlanner::with_defaults();
1186        dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0));
1187        dwa.set_goal(Point2D::new(10.0, 0.0));
1188        let control = dwa.try_plan_input().unwrap();
1189        assert!(control.v > 0.0);
1190    }
1191
1192    #[test]
1193    fn test_dwa_best_path_after_planning() {
1194        let mut dwa = DWAPlanner::with_defaults();
1195        dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0));
1196        dwa.set_goal(Point2D::new(3.0, 0.0));
1197        dwa.try_plan_step().unwrap();
1198        let best_path = dwa.best_path();
1199        assert!(!best_path.is_empty());
1200    }
1201
1202    #[test]
1203    fn test_legacy_obstacle_cost_checks_all_states_and_obstacles() {
1204        #[allow(deprecated)]
1205        let config = Config {
1206            max_speed: 1.0,
1207            min_speed: -0.5,
1208            max_yaw_rate: 40.0_f64.to_radians(),
1209            max_accel: 0.2,
1210            max_delta_yaw_rate: 40.0_f64.to_radians(),
1211            v_resolution: 0.01,
1212            yaw_rate_resolution: 0.1_f64.to_radians(),
1213            dt: 0.1,
1214            predict_time: 3.0,
1215            to_goal_cost_gain: 0.15,
1216            speed_cost_gain: 1.0,
1217            obstacle_cost_gain: 1.0,
1218            robot_type: DWARobotType::Circle,
1219            robot_radius: 0.5,
1220            robot_width: 0.5,
1221            robot_length: 1.2,
1222        };
1223        let trajectory = vec![
1224            Vector5::new(0.0, 0.0, 0.0, 0.0, 0.0),
1225            Vector5::new(2.0, 2.0, 0.0, 0.0, 0.0),
1226        ];
1227        let obstacles = [(10.0, 10.0), (2.0, 2.0)];
1228        #[allow(deprecated)]
1229        let cost = calc_obstacle_cost(&trajectory, &obstacles, &config);
1230        assert_eq!(cost, f64::MAX);
1231    }
1232
1233    #[test]
1234    fn an_all_colliding_window_stops_instead_of_picking_an_arc() {
1235        for gain in [0.3, 1.0, 2.0] {
1236            let mut dwa = DWAPlanner::new(DWAConfig {
1237                obstacle_cost_gain: gain,
1238                ..DWAConfig::default()
1239            });
1240            dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.5, 0.0));
1241            dwa.set_goal(Point2D::new(10.0, 0.0));
1242            // A ring of obstacles hugging the robot: every arc collides.
1243            dwa.set_obstacles(
1244                (0..36)
1245                    .map(|k| {
1246                        let a = k as f64 * std::f64::consts::TAU / 36.0;
1247                        Point2D::new(1.2 * a.cos(), 1.2 * a.sin())
1248                    })
1249                    .collect(),
1250            );
1251            let control = dwa.try_plan_step().expect("plan");
1252            assert_eq!(dwa.get_best_trajectory().cost, f64::MAX, "gain {gain}");
1253            assert_eq!(control[0], 0.0, "gain {gain}: drove at {}", control[0]);
1254        }
1255    }
1256
1257    #[test]
1258    fn test_dwa_rectangle_collision_detection_respects_local_frame() {
1259        let mut dwa = DWAPlanner::new(DWAConfig {
1260            robot_type: DWARobotType::Rectangle,
1261            robot_radius: 0.2,
1262            robot_width: 0.5,
1263            robot_length: 1.2,
1264            ..DWAConfig::default()
1265        });
1266        dwa.set_state(DWAState::new(
1267            0.0,
1268            0.0,
1269            std::f64::consts::FRAC_PI_2,
1270            0.0,
1271            0.0,
1272        ));
1273        dwa.set_obstacles(vec![Point2D::new(0.0, 0.55)]);
1274
1275        let trajectory = Trajectory {
1276            states: vec![*dwa.get_state()],
1277            control: DWAControl::zeros(),
1278            cost: 0.0,
1279        };
1280        let cost = dwa.calc_obstacle_cost(&trajectory);
1281        assert_eq!(cost, f64::MAX);
1282
1283        #[allow(deprecated)]
1284        let legacy_config = Config {
1285            max_speed: 1.0,
1286            min_speed: -0.5,
1287            max_yaw_rate: 40.0_f64.to_radians(),
1288            max_accel: 0.2,
1289            max_delta_yaw_rate: 40.0_f64.to_radians(),
1290            v_resolution: 0.01,
1291            yaw_rate_resolution: 0.1_f64.to_radians(),
1292            dt: 0.1,
1293            predict_time: 3.0,
1294            to_goal_cost_gain: 0.15,
1295            speed_cost_gain: 1.0,
1296            obstacle_cost_gain: 1.0,
1297            robot_type: DWARobotType::Rectangle,
1298            robot_radius: 0.2,
1299            robot_width: 0.5,
1300            robot_length: 1.2,
1301        };
1302        #[allow(deprecated)]
1303        let legacy_cost = calc_obstacle_cost(&trajectory.states, &[(0.0, 0.55)], &legacy_config);
1304        assert_eq!(legacy_cost, f64::MAX);
1305    }
1306
1307    #[test]
1308    fn test_dwa_default_main_initial_parity_matches_pythonrobotics() {
1309        let mut dwa = DWAPlanner::with_defaults();
1310        dwa.set_state(DWAState::new(
1311            0.0,
1312            0.0,
1313            std::f64::consts::FRAC_PI_8,
1314            0.0,
1315            0.0,
1316        ));
1317        dwa.set_goal(Point2D::new(10.0, 10.0));
1318        dwa.set_obstacles(pythonrobotics_default_obstacles());
1319
1320        let control = dwa.try_plan_step().unwrap();
1321        assert_close(control[0], 0.02);
1322        assert_close(control[1], 0.068_067_840_827_779_04);
1323
1324        let trajectory = &dwa.get_best_trajectory().states;
1325        assert_eq!(trajectory.len(), 31);
1326        assert_state_close(
1327            &trajectory[0],
1328            &[0.0, 0.0, std::f64::consts::FRAC_PI_8, 0.0, 0.0],
1329        );
1330        assert_state_close(
1331            &trajectory[1],
1332            &[
1333                0.001_842_506_612_952_407,
1334                0.000_777_926_334_061_682_8,
1335                0.399_505_865_781_502_05,
1336                0.02,
1337                0.068_067_840_827_779_04,
1338            ],
1339        );
1340        assert_state_close(
1341            &trajectory[2],
1342            &[
1343                0.003_679_675_406_577_515,
1344                0.001_568_376_094_470_316,
1345                0.406_312_649_864_279_95,
1346                0.02,
1347                0.068_067_840_827_779_04,
1348            ],
1349        );
1350        assert_state_close(
1351            &trajectory[5],
1352            &[
1353                0.009_158_304_932_256_946,
1354                0.004_014_496_810_584_791,
1355                0.426_733_002_112_613_67,
1356                0.02,
1357                0.068_067_840_827_779_04,
1358            ],
1359        );
1360        assert_state_close(
1361            &trajectory[10],
1362            &[
1363                0.018_174_703_618_670_7,
1364                0.008_338_301_686_158_353,
1365                0.460_766_922_526_503_2,
1366                0.02,
1367                0.068_067_840_827_779_04,
1368            ],
1369        );
1370        assert_state_close(
1371            &trajectory[20],
1372            &[
1373                0.035_740_187_678_676_45,
1374                0.017_893_451_940_005_592,
1375                0.528_834_763_354_282_3,
1376                0.02,
1377                0.068_067_840_827_779_04,
1378            ],
1379        );
1380        assert_state_close(
1381            &trajectory[30],
1382            &[
1383                0.052_615_098_653_616_07,
1384                0.028_621_196_634_198_105,
1385                0.596_902_604_182_061_3,
1386                0.02,
1387                0.068_067_840_827_779_04,
1388            ],
1389        );
1390
1391        #[allow(deprecated)]
1392        let legacy_config = Config {
1393            max_speed: 1.0,
1394            min_speed: -0.5,
1395            max_yaw_rate: 40.0_f64.to_radians(),
1396            max_accel: 0.2,
1397            max_delta_yaw_rate: 40.0_f64.to_radians(),
1398            v_resolution: 0.01,
1399            yaw_rate_resolution: 0.1_f64.to_radians(),
1400            dt: 0.1,
1401            predict_time: 3.0,
1402            to_goal_cost_gain: 0.15,
1403            speed_cost_gain: 1.0,
1404            obstacle_cost_gain: 1.0,
1405            robot_type: DWARobotType::Circle,
1406            robot_radius: 1.0,
1407            robot_width: 0.5,
1408            robot_length: 1.2,
1409        };
1410        let obstacle_tuples: Vec<_> = pythonrobotics_default_obstacles()
1411            .into_iter()
1412            .map(|point| (point.x, point.y))
1413            .collect();
1414        #[allow(deprecated)]
1415        let (legacy_u, legacy_trajectory) = dwa_control(
1416            DWAState::new(0.0, 0.0, std::f64::consts::FRAC_PI_8, 0.0, 0.0),
1417            &legacy_config,
1418            (10.0, 10.0),
1419            &obstacle_tuples,
1420        );
1421        assert_close(legacy_u.0, control[0]);
1422        assert_close(legacy_u.1, control[1]);
1423        assert_eq!(legacy_trajectory.len(), trajectory.len());
1424        assert_state_close(
1425            legacy_trajectory.last().unwrap(),
1426            &[
1427                0.052_615_098_653_616_07,
1428                0.028_621_196_634_198_105,
1429                0.596_902_604_182_061_3,
1430                0.02,
1431                0.068_067_840_827_779_04,
1432            ],
1433        );
1434    }
1435
1436    #[test]
1437    fn test_dwa_stuck_initial_control_matches_pythonrobotics() {
1438        let mut dwa = DWAPlanner::new(DWAConfig {
1439            to_goal_cost_gain: 0.2,
1440            obstacle_cost_gain: 2.0,
1441            ..DWAConfig::default()
1442        });
1443        dwa.set_state(DWAState::new(
1444            0.0,
1445            0.0,
1446            std::f64::consts::FRAC_PI_8,
1447            0.0,
1448            0.0,
1449        ));
1450        dwa.set_goal(Point2D::new(-5.0, -7.0));
1451        dwa.set_obstacles(pythonrobotics_stuck_obstacles());
1452
1453        let control = dwa.try_plan_step().unwrap();
1454        assert!(control[0].abs() < 1.0e-15);
1455        assert_close(control[1], -0.698_131_700_797_731_8);
1456
1457        let trajectory = &dwa.get_best_trajectory().states;
1458        assert_eq!(trajectory.len(), 31);
1459        assert_state_close(
1460            &trajectory[1],
1461            &[
1462                -3.214_541_934_276_051_4e-19,
1463                -1.305_290_122_999_866_4e-19,
1464                0.385_717_764_690_746_8,
1465                -3.469_446_951_953_614e-18,
1466                -0.069_813_170_079_773_18,
1467            ],
1468        );
1469        assert_state_close(
1470            &trajectory[30],
1471            &[
1472                -9.971_752_160_697_17e-18,
1473                -2.915_944_394_677_270_7e-18,
1474                0.183_259_571_459_404_3,
1475                -3.469_446_951_953_614e-18,
1476                -0.069_813_170_079_773_18,
1477            ],
1478        );
1479
1480        #[allow(deprecated)]
1481        let legacy_config = Config {
1482            max_speed: 1.0,
1483            min_speed: -0.5,
1484            max_yaw_rate: 40.0_f64.to_radians(),
1485            max_accel: 0.2,
1486            max_delta_yaw_rate: 40.0_f64.to_radians(),
1487            v_resolution: 0.01,
1488            yaw_rate_resolution: 0.1_f64.to_radians(),
1489            dt: 0.1,
1490            predict_time: 3.0,
1491            to_goal_cost_gain: 0.2,
1492            speed_cost_gain: 1.0,
1493            obstacle_cost_gain: 2.0,
1494            robot_type: DWARobotType::Circle,
1495            robot_radius: 1.0,
1496            robot_width: 0.5,
1497            robot_length: 1.2,
1498        };
1499        let obstacle_tuples: Vec<_> = pythonrobotics_stuck_obstacles()
1500            .into_iter()
1501            .map(|point| (point.x, point.y))
1502            .collect();
1503        #[allow(deprecated)]
1504        let (legacy_u, legacy_trajectory) = dwa_control(
1505            DWAState::new(0.0, 0.0, std::f64::consts::FRAC_PI_8, 0.0, 0.0),
1506            &legacy_config,
1507            (-5.0, -7.0),
1508            &obstacle_tuples,
1509        );
1510        assert!(legacy_u.0.abs() < 1.0e-15);
1511        assert_close(legacy_u.1, control[1]);
1512        assert_eq!(legacy_trajectory.len(), trajectory.len());
1513        assert_state_close(
1514            legacy_trajectory.last().unwrap(),
1515            &[
1516                -9.971_752_160_697_17e-18,
1517                -2.915_944_394_677_270_7e-18,
1518                0.183_259_571_459_404_3,
1519                -3.469_446_951_953_614e-18,
1520                -0.069_813_170_079_773_18,
1521            ],
1522        );
1523    }
1524
1525    #[test]
1526    fn test_dwa_rectangle_main_initial_parity_matches_pythonrobotics() {
1527        let mut dwa = DWAPlanner::new(DWAConfig {
1528            robot_type: DWARobotType::Rectangle,
1529            ..DWAConfig::default()
1530        });
1531        dwa.set_state(DWAState::new(
1532            0.0,
1533            0.0,
1534            std::f64::consts::FRAC_PI_8,
1535            0.0,
1536            0.0,
1537        ));
1538        dwa.set_goal(Point2D::new(1.0, 1.0));
1539        dwa.set_obstacles(pythonrobotics_default_obstacles());
1540
1541        let control = dwa.try_plan_step().unwrap();
1542        assert_close(control[0], 0.02);
1543        assert_close(control[1], 0.068_067_840_827_779_04);
1544
1545        let trajectory = &dwa.get_best_trajectory().states;
1546        assert_eq!(trajectory.len(), 31);
1547        assert_state_close(
1548            &trajectory[1],
1549            &[
1550                0.001_842_506_612_952_407,
1551                0.000_777_926_334_061_682_8,
1552                0.399_505_865_781_502_05,
1553                0.02,
1554                0.068_067_840_827_779_04,
1555            ],
1556        );
1557        assert_state_close(
1558            &trajectory[30],
1559            &[
1560                0.052_615_098_653_616_07,
1561                0.028_621_196_634_198_105,
1562                0.596_902_604_182_061_3,
1563                0.02,
1564                0.068_067_840_827_779_04,
1565            ],
1566        );
1567
1568        #[allow(deprecated)]
1569        let legacy_config = Config {
1570            max_speed: 1.0,
1571            min_speed: -0.5,
1572            max_yaw_rate: 40.0_f64.to_radians(),
1573            max_accel: 0.2,
1574            max_delta_yaw_rate: 40.0_f64.to_radians(),
1575            v_resolution: 0.01,
1576            yaw_rate_resolution: 0.1_f64.to_radians(),
1577            dt: 0.1,
1578            predict_time: 3.0,
1579            to_goal_cost_gain: 0.15,
1580            speed_cost_gain: 1.0,
1581            obstacle_cost_gain: 1.0,
1582            robot_type: DWARobotType::Rectangle,
1583            robot_radius: 1.0,
1584            robot_width: 0.5,
1585            robot_length: 1.2,
1586        };
1587        let obstacle_tuples: Vec<_> = pythonrobotics_default_obstacles()
1588            .into_iter()
1589            .map(|point| (point.x, point.y))
1590            .collect();
1591        #[allow(deprecated)]
1592        let (legacy_u, legacy_trajectory) = dwa_control(
1593            DWAState::new(0.0, 0.0, std::f64::consts::FRAC_PI_8, 0.0, 0.0),
1594            &legacy_config,
1595            (1.0, 1.0),
1596            &obstacle_tuples,
1597        );
1598        assert_close(legacy_u.0, control[0]);
1599        assert_close(legacy_u.1, control[1]);
1600        assert_eq!(legacy_trajectory.len(), trajectory.len());
1601        assert_state_close(
1602            legacy_trajectory.last().unwrap(),
1603            &[
1604                0.052_615_098_653_616_07,
1605                0.028_621_196_634_198_105,
1606                0.596_902_604_182_061_3,
1607                0.02,
1608                0.068_067_840_827_779_04,
1609            ],
1610        );
1611    }
1612
1613    #[test]
1614    fn test_dwa_default_closed_loop_matches_pythonrobotics_main() {
1615        let mut dwa = DWAPlanner::with_defaults();
1616        dwa.set_state(DWAState::new(
1617            0.0,
1618            0.0,
1619            std::f64::consts::FRAC_PI_8,
1620            0.0,
1621            0.0,
1622        ));
1623        dwa.set_goal(Point2D::new(10.0, 10.0));
1624        dwa.set_obstacles(pythonrobotics_default_obstacles());
1625
1626        let trace = run_closed_loop_trace(dwa, 1_000).unwrap();
1627        assert_eq!(trace.len(), 221);
1628        let expected = [
1629            (
1630                0,
1631                ClosedLoopSample {
1632                    control: [0.02, 0.068_067_840_827_779_04],
1633                    state: [
1634                        0.001_842_506_612_952_407,
1635                        0.000_777_926_334_061_682_8,
1636                        0.399_505_865_781_502_05,
1637                        0.02,
1638                        0.068_067_840_827_779_04,
1639                    ],
1640                    predicted_len: 31,
1641                    predicted_last: [
1642                        0.052_615_098_653_616_07,
1643                        0.028_621_196_634_198_105,
1644                        0.596_902_604_182_061_3,
1645                        0.02,
1646                        0.068_067_840_827_779_04,
1647                    ],
1648                    dist: 14.140_282_717_861_751,
1649                },
1650            ),
1651            (
1652                1,
1653                ClosedLoopSample {
1654                    control: [0.04, 0.129_154_364_647_580_6],
1655                    state: [
1656                        0.005_507_118_539_734_564,
1657                        0.002_381_241_470_298_463_5,
1658                        0.412_421_302_246_260_1,
1659                        0.04,
1660                        0.129_154_364_647_580_6,
1661                    ],
1662                    predicted_len: 31,
1663                    predicted_last: [
1664                        0.100_285_621_194_050_45,
1665                        0.068_082_434_446_487_08,
1666                        0.786_968_959_724_242_9,
1667                        0.04,
1668                        0.129_154_364_647_580_6,
1669                    ],
1670                    dist: 14.136_557_883_673_978,
1671                },
1672            ),
1673            (
1674                2,
1675                ClosedLoopSample {
1676                    control: [0.06, 0.125_663_706_143_592],
1677                    state: [
1678                        0.010_973_381_433_457_923,
1679                        0.004_855_098_777_562_393,
1680                        0.424_987_672_860_619_34,
1681                        0.06,
1682                        0.125_663_706_143_592,
1683                    ],
1684                    predicted_len: 31,
1685                    predicted_last: [
1686                        0.152_459_040_871_135_35,
1687                        0.104_477_455_550_390_23,
1688                        0.789_412_420_677_035_5,
1689                        0.06,
1690                        0.125_663_706_143_592,
1691                    ],
1692                    dist: 14.130_943_860_296_941,
1693                },
1694            ),
1695            (
1696                10,
1697                ClosedLoopSample {
1698                    control: [0.209_999_999_999_999_96, 0.097_738_438_111_683_48],
1699                    state: [
1700                        0.113_698_261_449_284_92,
1701                        0.058_672_484_225_696_07,
1702                        0.511_905_069_609_937_5,
1703                        0.209_999_999_999_999_96,
1704                        0.097_738_438_111_683_48,
1705                    ],
1706                    predicted_len: 31,
1707                    predicted_last: [
1708                        0.593_749_519_151_196_6,
1709                        0.430_099_924_235_568_86,
1710                        0.795_346_540_133_818_3,
1711                        0.209_999_999_999_999_96,
1712                        0.097_738_438_111_683_48,
1713                    ],
1714                    dist: 14.020_305_090_887_366,
1715                },
1716            ),
1717            (
1718                50,
1719                ClosedLoopSample {
1720                    control: [0.990_000_000_000_000_7, 0.054_105_206_811_826_91],
1721                    state: [
1722                        1.950_167_034_188_733_6,
1723                        1.694_578_822_612_822_2,
1724                        0.921_882_910_903_411_1,
1725                        0.990_000_000_000_000_7,
1726                        0.054_105_206_811_826_91,
1727                    ],
1728                    predicted_len: 31,
1729                    predicted_last: [
1730                        3.492_440_509_739_950_5,
1731                        4.112_668_327_825_561,
1732                        1.078_788_010_657_709_2,
1733                        0.990_000_000_000_000_7,
1734                        0.054_105_206_811_826_91,
1735                    ],
1736                    dist: 11.566_323_171_658_883,
1737                },
1738            ),
1739            (
1740                100,
1741                ClosedLoopSample {
1742                    control: [0.990_000_000_000_000_7, 0.078_539_816_339_751_13],
1743                    state: [
1744                        3.530_364_817_697_800_6,
1745                        6.300_822_976_885_286,
1746                        1.472_883_355_758_044_3,
1747                        0.990_000_000_000_000_7,
1748                        0.078_539_816_339_751_13,
1749                    ],
1750                    predicted_len: 31,
1751                    predicted_last: [
1752                        3.473_368_247_999_231,
1753                        9.165_061_521_908_662,
1754                        1.700_648_823_143_322_3,
1755                        0.990_000_000_000_000_7,
1756                        0.078_539_816_339_751_13,
1757                    ],
1758                    dist: 7.452_522_394_493_172,
1759                },
1760            ),
1761            (
1762                150,
1763                ClosedLoopSample {
1764                    control: [0.990_000_000_000_000_7, -0.064_577_182_323_779_74],
1765                    state: [
1766                        5.167_079_306_155_943,
1767                        10.598_045_278_751_194,
1768                        0.561_297_887_441_443_9,
1769                        0.990_000_000_000_000_7,
1770                        -0.064_577_182_323_779_74,
1771                    ],
1772                    predicted_len: 31,
1773                    predicted_last: [
1774                        7.730_226_783_306_823,
1775                        11.882_133_506_527_275,
1776                        0.374_024_058_702_483_34,
1777                        0.990_000_000_000_000_7,
1778                        -0.064_577_182_323_779_74,
1779                    ],
1780                    dist: 4.869_782_396_413_901_5,
1781                },
1782            ),
1783            (
1784                200,
1785                ClosedLoopSample {
1786                    control: [0.990_000_000_000_000_7, -0.383_972_435_438_737_94],
1787                    state: [
1788                        9.503_213_445_849_001,
1789                        12.241_991_336_274_216,
1790                        -0.511_730_536_684_608_7,
1791                        0.990_000_000_000_000_7,
1792                        -0.383_972_435_438_737_94,
1793                    ],
1794                    predicted_len: 31,
1795                    predicted_last: [
1796                        10.769_122_970_663_686,
1797                        9.828_742_617_084_059,
1798                        -1.625_250_599_456_947_5,
1799                        0.990_000_000_000_000_7,
1800                        -0.383_972_435_438_737_94,
1801                    ],
1802                    dist: 2.296_371_492_662_689_5,
1803                },
1804            ),
1805            (
1806                220,
1807                ClosedLoopSample {
1808                    control: [0.990_000_000_000_000_7, -0.452_040_276_266_514_66],
1809                    state: [
1810                        10.682_812_904_798_725,
1811                        10.723_560_741_508_503,
1812                        -1.339_365_667_980_288_6,
1813                        0.990_000_000_000_000_7,
1814                        -0.452_040_276_266_514_66,
1815                    ],
1816                    predicted_len: 31,
1817                    predicted_last: [
1818                        9.529_554_489_209_27,
1819                        8.315_446_240_490_422,
1820                        -2.650_282_469_153_183,
1821                        0.990_000_000_000_000_7,
1822                        -0.452_040_276_266_514_66,
1823                    ],
1824                    dist: 0.994_873_665_151_514_7,
1825                },
1826            ),
1827        ];
1828        for (index, expected) in expected {
1829            assert_sample_close(&trace[index], &expected);
1830        }
1831    }
1832
1833    #[test]
1834    #[ignore = "long-running closed-loop regression scenario"]
1835    fn test_dwa_stuck_closed_loop_matches_pythonrobotics_reference() {
1836        let mut dwa = DWAPlanner::new(DWAConfig {
1837            to_goal_cost_gain: 0.2,
1838            obstacle_cost_gain: 2.0,
1839            ..DWAConfig::default()
1840        });
1841        dwa.set_state(DWAState::new(
1842            0.0,
1843            0.0,
1844            std::f64::consts::FRAC_PI_8,
1845            0.0,
1846            0.0,
1847        ));
1848        dwa.set_goal(Point2D::new(-5.0, -7.0));
1849        dwa.set_obstacles(pythonrobotics_stuck_obstacles());
1850
1851        let trace = run_closed_loop_trace(dwa, 1_000).unwrap();
1852        assert_eq!(trace.len(), 461);
1853        let expected = [
1854            (
1855                0,
1856                ClosedLoopSample {
1857                    control: [-3.469_446_951_953_614e-18, -0.698_131_700_797_731_8],
1858                    state: [
1859                        -3.290_158_615_020_658_6e-19,
1860                        -1.100_871_673_005_335_8e-19,
1861                        0.322_885_911_618_950_97,
1862                        -3.469_446_951_953_614e-18,
1863                        -0.698_131_700_797_731_8,
1864                    ],
1865                    predicted_len: 31,
1866                    predicted_last: [
1867                        -9.971_752_160_697_17e-18,
1868                        -2.915_944_394_677_270_7e-18,
1869                        0.183_259_571_459_404_3,
1870                        -3.469_446_951_953_614e-18,
1871                        -0.069_813_170_079_773_18,
1872                    ],
1873                    dist: 8.602_325_267_042_627,
1874                },
1875            ),
1876            (
1877                1,
1878                ClosedLoopSample {
1879                    control: [0.019_999_999_999_999_993, -0.698_131_700_797_731_8],
1880                    state: [
1881                        0.001_936_295_280_756_214_5,
1882                        0.000_500_760_008_108_882_6,
1883                        0.253_072_741_539_177_8,
1884                        0.019_999_999_999_999_993,
1885                        -0.698_131_700_797_731_8,
1886                    ],
1887                    predicted_len: 31,
1888                    predicted_last: [
1889                        0.036_000_096_958_749_65,
1890                        -0.034_162_816_412_153_525,
1891                        -1.771_509_190_774_245_5,
1892                        0.019_999_999_999_999_993,
1893                        -0.698_131_700_797_731_8,
1894                    ],
1895                    dist: 8.603_858_296_887_57,
1896                },
1897            ),
1898            (
1899                2,
1900                ClosedLoopSample {
1901                    control: [0.039_999_999_999_999_994, -0.698_131_700_797_731_8],
1902                    state: [
1903                        0.005_869_314_911_012_033,
1904                        0.001_229_702_110_077_472_6,
1905                        0.183_259_571_459_404_64,
1906                        0.039_999_999_999_999_994,
1907                        -0.698_131_700_797_731_8,
1908                    ],
1909                    predicted_len: 31,
1910                    predicted_last: [
1911                        0.068_994_945_132_430_06,
1912                        -0.072_680_914_645_250_44,
1913                        -1.841_322_360_854_018_8,
1914                        0.039_999_999_999_999_994,
1915                        -0.698_131_700_797_731_8,
1916                    ],
1917                    dist: 8.606_738_345_022_23,
1918                },
1919            ),
1920            (
1921                10,
1922                ClosedLoopSample {
1923                    control: [0.189_999_999_999_999_95, -0.647_517_152_489_896_2],
1924                    state: [
1925                        0.104_132_970_400_981_02,
1926                        -0.016_809_261_291_929_67,
1927                        -0.366_344_609_993_609_74,
1928                        0.189_999_999_999_999_95,
1929                        -0.647_517_152_489_896_2,
1930                    ],
1931                    predicted_len: 31,
1932                    predicted_last: [
1933                        0.213_572_392_076_469_75,
1934                        -0.477_614_640_782_877_6,
1935                        -2.244_144_352_214_308_3,
1936                        0.189_999_999_999_999_95,
1937                        -0.647_517_152_489_896_2,
1938                    ],
1939                    dist: 8.649_689_374_348_22,
1940                },
1941            ),
1942            (
1943                50,
1944                ClosedLoopSample {
1945                    control: [0.029_999_999_999_999_79, -0.167_551_608_191_450_54],
1946                    state: [
1947                        0.343_298_669_611_588_45,
1948                        -0.402_879_787_203_639_7,
1949                        -1.769_938_394_447_439,
1950                        0.029_999_999_999_999_79,
1951                        -0.167_551_608_191_450_54,
1952                    ],
1953                    predicted_len: 31,
1954                    predicted_last: [
1955                        0.305_790_986_927_416_24,
1956                        -0.480_433_609_151_910_36,
1957                        -2.255_838_058_202_643,
1958                        0.029_999_999_999_999_79,
1959                        -0.167_551_608_191_450_54,
1960                    ],
1961                    dist: 8.489_572_178_547,
1962                },
1963            ),
1964            (
1965                100,
1966                ClosedLoopSample {
1967                    control: [0.269_999_999_999_999_7, -0.005_235_987_755_980_706],
1968                    state: [
1969                        0.106_092_820_485_199_51,
1970                        -0.538_702_831_822_151_7,
1971                        -2.837_905_363_742_752,
1972                        0.269_999_999_999_999_7,
1973                        -0.005_235_987_755_980_706,
1974                    ],
1975                    predicted_len: 31,
1976                    predicted_last: [
1977                        -0.642_886_107_988_420_7,
1978                        -0.766_974_159_554_404_4,
1979                        -2.853_089_728_235_101_8,
1980                        0.269_999_999_999_999_7,
1981                        -0.005_235_987_755_980_706,
1982                    ],
1983                    dist: 8.235_323_004_406_906,
1984                },
1985            ),
1986            (
1987                200,
1988                ClosedLoopSample {
1989                    control: [0.990_000_000_000_000_3, 0.308_923_277_603_009_64],
1990                    state: [
1991                        1.852_193_706_436_120_7,
1992                        -4.220_215_843_560_918,
1993                        0.757_647_428_290_844_2,
1994                        0.990_000_000_000_000_3,
1995                        0.308_923_277_603_009_64,
1996                    ],
1997                    predicted_len: 31,
1998                    predicted_last: [
1999                        2.803_482_546_405_410_5,
2000                        -1.612_237_727_831_649_8,
2001                        1.653_524_933_339_573_6,
2002                        0.990_000_000_000_000_3,
2003                        0.308_923_277_603_009_64,
2004                    ],
2005                    dist: 7.394_576_292_588_543,
2006                },
2007            ),
2008            (
2009                300,
2010                ClosedLoopSample {
2011                    control: [0.990_000_000_000_000_3, 0.371_755_130_674_814_8],
2012                    state: [
2013                        0.769_215_774_844_203_3,
2014                        3.386_587_574_598_417_3,
2015                        3.243_519_881_906_541,
2016                        0.990_000_000_000_000_3,
2017                        0.371_755_130_674_814_8,
2018                    ],
2019                    predicted_len: 31,
2020                    predicted_last: [
2021                        -1.391_460_065_500_792_3,
2022                        1.711_199_521_041_678_8,
2023                        4.321_609_760_863_501,
2024                        0.990_000_000_000_000_3,
2025                        0.371_755_130_674_814_8,
2026                    ],
2027                    dist: 11.881_290_001_574_468,
2028                },
2029            ),
2030            (
2031                400,
2032                ClosedLoopSample {
2033                    control: [0.299_999_999_999_999_7, 0.118_682_389_135_646_52],
2034                    state: [
2035                        -4.821_582_173_842_302,
2036                        -2.257_266_302_545_886_6,
2037                        4.841_543_345_032_829,
2038                        0.299_999_999_999_999_7,
2039                        0.118_682_389_135_646_52,
2040                    ],
2041                    predicted_len: 31,
2042                    predicted_last: [
2043                        -4.559_815_331_068_457,
2044                        -3.082_459_945_503_760_2,
2045                        5.185_722_273_526_192,
2046                        0.299_999_999_999_999_7,
2047                        0.118_682_389_135_646_52,
2048                    ],
2049                    dist: 4.746_088_478_490_219,
2050                },
2051            ),
2052            (
2053                460,
2054                ClosedLoopSample {
2055                    control: [0.990_000_000_000_000_3, -0.506_145_483_078_318_9],
2056                    state: [
2057                        -4.985_352_164_947_927,
2058                        -6.059_407_170_506_825,
2059                        4.171_685_978_117_614,
2060                        0.990_000_000_000_000_3,
2061                        -0.506_145_483_078_318_9,
2062                    ],
2063                    predicted_len: 31,
2064                    predicted_last: [
2065                        -7.510_205_006_513_761,
2066                        -6.760_560_048_330_578_6,
2067                        2.703_864_077_190_488,
2068                        0.990_000_000_000_000_3,
2069                        -0.506_145_483_078_318_9,
2070                    ],
2071                    dist: 0.940_706_877_813_535_5,
2072                },
2073            ),
2074        ];
2075        for (index, expected) in expected {
2076            assert_sample_close(&trace[index], &expected);
2077        }
2078    }
2079
2080    #[test]
2081    fn test_dwa_rectangle_closed_loop_matches_pythonrobotics_main2() {
2082        let mut dwa = DWAPlanner::new(DWAConfig {
2083            robot_type: DWARobotType::Rectangle,
2084            ..DWAConfig::default()
2085        });
2086        dwa.set_state(DWAState::new(
2087            0.0,
2088            0.0,
2089            std::f64::consts::FRAC_PI_8,
2090            0.0,
2091            0.0,
2092        ));
2093        dwa.set_goal(Point2D::new(1.0, 1.0));
2094        dwa.set_obstacles(pythonrobotics_default_obstacles());
2095
2096        let trace = run_closed_loop_trace(dwa, 1_000).unwrap();
2097        assert!((21..=22).contains(&trace.len()));
2098        assert!(trace.last().unwrap().dist <= 1.05);
2099        let expected = [
2100            (
2101                0,
2102                ClosedLoopSample {
2103                    control: [0.02, 0.068_067_840_827_779_04],
2104                    state: [
2105                        0.001_842_506_612_952_407,
2106                        0.000_777_926_334_061_682_8,
2107                        0.399_505_865_781_502_05,
2108                        0.02,
2109                        0.068_067_840_827_779_04,
2110                    ],
2111                    predicted_len: 31,
2112                    predicted_last: [
2113                        0.052_615_098_653_616_07,
2114                        0.028_621_196_634_198_105,
2115                        0.596_902_604_182_061_3,
2116                        0.02,
2117                        0.068_067_840_827_779_04,
2118                    ],
2119                    dist: 1.412_360_837_075_983_3,
2120                },
2121            ),
2122            (
2123                1,
2124                ClosedLoopSample {
2125                    control: [0.04, 0.134_390_352_403_563_56],
2126                    state: [
2127                        0.005_506_278_543_593_619,
2128                        0.002_383_160_036_749_725_7,
2129                        0.412_944_901_021_858_4,
2130                        0.04,
2131                        0.134_390_352_403_563_56,
2132                    ],
2133                    predicted_len: 31,
2134                    predicted_last: [
2135                        0.099_685_436_565_874_95,
2136                        0.068_843_867_784_433_95,
2137                        0.802_676_922_992_193_3,
2138                        0.04,
2139                        0.134_390_352_403_563_56,
2140                    ],
2141                    dist: 1.408_636_617_937_526_7,
2142                },
2143            ),
2144            (
2145                2,
2146                ClosedLoopSample {
2147                    control: [0.06, 0.132_645_023_151_569_34],
2148                    state: [
2149                        0.010_969_514_971_661_525,
2150                        0.004_863_693_796_050_7,
2151                        0.426_209_403_337_015_3,
2152                        0.06,
2153                        0.132_645_023_151_569_34,
2154                    ],
2155                    predicted_len: 31,
2156                    predicted_last: [
2157                        0.151_191_833_116_684_02,
2158                        0.106_069_645_038_751_56,
2159                        0.810_879_970_476_567,
2160                        0.06,
2161                        0.132_645_023_151_569_34,
2162                    ],
2163                    dist: 1.403_024_436_081_079_3,
2164                },
2165            ),
2166            (
2167                5,
2168                ClosedLoopSample {
2169                    control: [0.119_999_999_999_999_95, 0.129_154_364_647_581],
2170                    state: [
2171                        0.037_928_184_668_002_49,
2172                        0.018_021_637_577_041_828,
2173                        0.465_304_778_581_688_45,
2174                        0.119_999_999_999_999_95,
2175                        0.129_154_364_647_581,
2176                    ],
2177                    predicted_len: 31,
2178                    predicted_last: [
2179                        0.311_447_525_499_293_9,
2180                        0.229_879_307_030_410_07,
2181                        0.839_852_436_059_674,
2182                        0.119_999_999_999_999_95,
2183                        0.129_154_364_647_581,
2184                    ],
2185                    dist: 1.374_723_129_260_244_9,
2186                },
2187            ),
2188            (
2189                10,
2190                ClosedLoopSample {
2191                    control: [0.209_999_999_999_999_96, 0.127_409_035_395_587_23],
2192                    state: [
2193                        0.113_124_079_420_109_7,
2194                        0.059_725_368_755_827_526,
2195                        0.529_183_829_204_681_3,
2196                        0.209_999_999_999_999_96,
2197                        0.127_409_035_395_587_23,
2198                    ],
2199                    predicted_len: 31,
2200                    predicted_last: [
2201                        0.568_258_124_617_134_2,
2202                        0.459_148_358_042_203_83,
2203                        0.898_670_031_851_883_7,
2204                        0.209_999_999_999_999_96,
2205                        0.127_409_035_395_587_23,
2206                    ],
2207                    dist: 1.292_542_177_519_090_6,
2208                },
2209            ),
2210            (
2211                20,
2212                ClosedLoopSample {
2213                    control: [0.340_000_000_000_000_1, 0.139_626_340_159_548_67],
2214                    state: [
2215                        0.365_710_870_049_402_1,
2216                        0.235_568_742_831_355_18,
2217                        0.666_192_175_486_237_9,
2218                        0.340_000_000_000_000_1,
2219                        0.139_626_340_159_548_67,
2220                    ],
2221                    predicted_len: 31,
2222                    predicted_last: [
2223                        0.992_955_085_115_716,
2224                        0.987_609_593_409_779_3,
2225                        1.071_108_561_948_929_3,
2226                        0.340_000_000_000_000_1,
2227                        0.139_626_340_159_548_67,
2228                    ],
2229                    dist: 0.993_316_589_668_128_5,
2230                },
2231            ),
2232        ];
2233        for (index, expected) in expected {
2234            assert_sample_close(&trace[index], &expected);
2235        }
2236    }
2237}