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