Skip to main content

rust_robotics_control/
rear_wheel_feedback.rs

1//! Rear Wheel Feedback path tracking controller
2//!
3//! A path tracking controller based on rear wheel feedback steering control
4//! that uses heading error and lateral error for computing the steering angle.
5//!
6//! Ref:
7//!     - PythonRobotics: <https://github.com/AtsushiSakai/PythonRobotics>
8//!     - B. Paden, M. Cap, S. Z. Yong, D. Yershov and E. Frazzoli,
9//!       "A Survey of Motion Planning and Control Techniques Adopted in Self-Driving Vehicles"
10
11use crate::spline_course::calc_spline_course;
12use rust_robotics_core::normalize_angle;
13use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
14
15/// Vehicle state for Rear Wheel Feedback Controller
16#[derive(Debug, Clone, Copy)]
17pub struct VehicleState {
18    pub x: f64,
19    pub y: f64,
20    pub yaw: f64,
21    pub v: f64,
22    pub wheelbase: f64,
23}
24
25impl VehicleState {
26    pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64) -> Self {
27        VehicleState {
28            x,
29            y,
30            yaw,
31            v,
32            wheelbase,
33        }
34    }
35
36    /// Update vehicle state using bicycle model kinematics
37    pub fn update(&mut self, a: f64, delta: f64, dt: f64) {
38        self.x += self.v * self.yaw.cos() * dt;
39        self.y += self.v * self.yaw.sin() * dt;
40        self.yaw += self.v / self.wheelbase * delta.tan() * dt;
41        self.v += a * dt;
42    }
43
44    /// Get rear axle position (same as state position for rear wheel reference)
45    pub fn rear_axle(&self) -> (f64, f64) {
46        (self.x, self.y)
47    }
48
49    pub fn to_state2d(&self) -> State2D {
50        State2D::new(self.x, self.y, self.yaw, self.v)
51    }
52}
53
54impl From<State2D> for VehicleState {
55    fn from(s: State2D) -> Self {
56        VehicleState::new(s.x, s.y, s.yaw, s.v, 2.9) // default wheelbase
57    }
58}
59
60/// Configuration for Rear Wheel Feedback Controller
61#[derive(Debug, Clone)]
62pub struct RearWheelFeedbackConfig {
63    /// Heading error gain (KTH)
64    pub kth: f64,
65    /// Lateral error gain (KE)
66    pub ke: f64,
67    /// Vehicle wheelbase \[m\]
68    pub wheelbase: f64,
69    /// Speed proportional gain
70    pub kp: f64,
71    /// Goal distance threshold \[m\]
72    pub goal_threshold: f64,
73    /// Maximum steering angle \[rad\]
74    pub max_steer: f64,
75}
76
77impl Default for RearWheelFeedbackConfig {
78    fn default() -> Self {
79        Self {
80            kth: 1.0,
81            ke: 0.5,
82            wheelbase: 2.9,
83            kp: 1.0,
84            goal_threshold: 0.5,
85            max_steer: 45.0_f64.to_radians(),
86        }
87    }
88}
89
90/// Rear Wheel Feedback path tracking controller
91pub struct RearWheelFeedbackController {
92    config: RearWheelFeedbackConfig,
93    path: Path2D,
94    path_yaw: Vec<f64>,
95    path_curvature: Vec<f64>,
96    last_target_idx: usize,
97}
98
99impl RearWheelFeedbackController {
100    /// Create a new Rear Wheel Feedback controller
101    pub fn new(config: RearWheelFeedbackConfig) -> Self {
102        RearWheelFeedbackController {
103            config,
104            path: Path2D::new(),
105            path_yaw: Vec::new(),
106            path_curvature: Vec::new(),
107            last_target_idx: 0,
108        }
109    }
110
111    /// Create with simplified parameters (legacy interface)
112    pub fn with_params(kth: f64, ke: f64, wheelbase: f64) -> Self {
113        let config = RearWheelFeedbackConfig {
114            kth,
115            ke,
116            wheelbase,
117            ..Default::default()
118        };
119        Self::new(config)
120    }
121
122    /// Set the reference path
123    pub fn set_path(&mut self, path: Path2D) {
124        self.path_yaw = self.compute_path_yaw(&path);
125        self.path_curvature = self.compute_path_curvature(&path);
126        self.path = path;
127        self.last_target_idx = 0;
128    }
129
130    /// Set the reference path with pre-computed yaw angles and curvatures
131    pub fn set_path_with_info(&mut self, path: Path2D, yaw: Vec<f64>, curvature: Vec<f64>) {
132        self.path = path;
133        self.path_yaw = yaw;
134        self.path_curvature = curvature;
135        self.last_target_idx = 0;
136    }
137
138    /// Get the current reference path
139    pub fn get_path(&self) -> &Path2D {
140        &self.path
141    }
142
143    /// Compute yaw angles from path points
144    fn compute_path_yaw(&self, path: &Path2D) -> Vec<f64> {
145        path.yaw_profile()
146    }
147
148    /// Compute curvature from path points
149    fn compute_path_curvature(&self, path: &Path2D) -> Vec<f64> {
150        let n = path.len();
151        if n < 3 {
152            return vec![0.0; n];
153        }
154
155        let mut curvature = Vec::with_capacity(n);
156
157        // First point
158        curvature.push(0.0);
159
160        // Middle points - compute curvature using three consecutive points
161        for i in 1..n - 1 {
162            let p0 = &path.points[i - 1];
163            let p1 = &path.points[i];
164            let p2 = &path.points[i + 1];
165
166            let dx1 = p1.x - p0.x;
167            let dy1 = p1.y - p0.y;
168            let dx2 = p2.x - p1.x;
169            let dy2 = p2.y - p1.y;
170
171            let ds1 = (dx1 * dx1 + dy1 * dy1).sqrt();
172            let ds2 = (dx2 * dx2 + dy2 * dy2).sqrt();
173
174            if ds1 > 1e-6 && ds2 > 1e-6 {
175                let yaw1 = dy1.atan2(dx1);
176                let yaw2 = dy2.atan2(dx2);
177                let dyaw = normalize_angle(yaw2 - yaw1);
178                let ds = (ds1 + ds2) / 2.0;
179                curvature.push(dyaw / ds);
180            } else {
181                curvature.push(0.0);
182            }
183        }
184
185        // Last point
186        curvature.push(*curvature.last().unwrap_or(&0.0));
187
188        curvature
189    }
190
191    /// Find target index and compute lateral error
192    fn calc_target_index(&self, state: &VehicleState) -> (usize, f64) {
193        let (rx, ry) = state.rear_axle();
194        let query = Point2D::new(rx, ry);
195        let min_idx = self
196            .path
197            .nearest_point_index_from(query, self.last_target_idx)
198            .unwrap_or(0);
199
200        // Calculate lateral error (signed distance from path)
201        let target_point = &self.path.points[min_idx];
202        let dx = rx - target_point.x;
203        let dy = ry - target_point.y;
204
205        // Lateral error with sign (positive = left of path, negative = right)
206        let path_yaw = if min_idx < self.path_yaw.len() {
207            self.path_yaw[min_idx]
208        } else {
209            0.0
210        };
211
212        // Cross product to determine sign
213        let error = -path_yaw.sin() * dx + path_yaw.cos() * dy;
214
215        (min_idx, error)
216    }
217
218    /// Compute steering angle using rear wheel feedback control law
219    ///
220    /// Control law:
221    /// omega = v * k * cos(th_e) / (1.0 - k * e) - KTH * |v| * th_e - KE * v * sin(th_e) * e / th_e
222    /// delta = atan2(L * omega / v, 1.0)
223    pub fn compute_steering(&mut self, state: &VehicleState) -> f64 {
224        if self.path.is_empty() {
225            return 0.0;
226        }
227
228        let (mut target_idx, e) = self.calc_target_index(state);
229
230        // Don't go backwards on path
231        if self.last_target_idx >= target_idx {
232            target_idx = self.last_target_idx;
233        }
234        self.last_target_idx = target_idx;
235
236        // Get path yaw and curvature at target point
237        let yaw_ref = if target_idx < self.path_yaw.len() {
238            self.path_yaw[target_idx]
239        } else {
240            *self.path_yaw.last().unwrap_or(&0.0)
241        };
242
243        let k = if target_idx < self.path_curvature.len() {
244            self.path_curvature[target_idx]
245        } else {
246            *self.path_curvature.last().unwrap_or(&0.0)
247        };
248
249        // Heading error
250        let th_e = normalize_angle(state.yaw - yaw_ref);
251
252        // Compute angular velocity using rear wheel feedback control law
253        let v = state.v;
254        let v_abs = v.abs().max(0.1); // Avoid division by zero
255
256        // sin(th_e) / th_e tends to 1 as th_e -> 0; evaluating it naively there
257        // would drop the lateral-error term exactly when the heading is aligned.
258        let sinc = if th_e.abs() < 1e-6 {
259            1.0
260        } else {
261            th_e.sin() / th_e
262        };
263
264        // Rear wheel feedback control formula
265        let denom = 1.0 - k * e;
266        let denom_safe = if denom.abs() < 0.01 {
267            0.01_f64.copysign(denom)
268        } else {
269            denom
270        };
271
272        let omega = v * k * th_e.cos() / denom_safe
273            - self.config.kth * v_abs * th_e
274            - self.config.ke * v * sinc * e;
275
276        // Compute steering angle
277        let delta = if v.abs() > 0.01 {
278            (self.config.wheelbase * omega / v).atan()
279        } else {
280            0.0
281        };
282
283        // Clamp to maximum steering angle
284        delta.clamp(-self.config.max_steer, self.config.max_steer)
285    }
286
287    /// Proportional speed control
288    pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
289        self.config.kp * (target_speed - current_speed)
290    }
291
292    /// Check if goal is reached
293    pub fn is_goal_reached_vehicle(&self, state: &VehicleState) -> bool {
294        if let Some(goal) = self.path.points.last() {
295            let dx = state.x - goal.x;
296            let dy = state.y - goal.y;
297            (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
298        } else {
299            true
300        }
301    }
302
303    /// Legacy planning interface - simulate path tracking
304    pub fn planning(
305        &mut self,
306        waypoints: Vec<(f64, f64)>,
307        target_speed: f64,
308        ds: f64,
309    ) -> Option<Vec<(f64, f64)>> {
310        if waypoints.len() < 2 {
311            return None;
312        }
313
314        // Generate spline path
315        let ax: Vec<f64> = waypoints.iter().map(|p| p.0).collect();
316        let ay: Vec<f64> = waypoints.iter().map(|p| p.1).collect();
317        let (cx, cy, cyaw, ck, _) = calc_spline_course(&ax, &ay, ds);
318
319        // Set path
320        let path = Path2D::from_points(
321            cx.iter()
322                .zip(cy.iter())
323                .map(|(&x, &y)| Point2D::new(x, y))
324                .collect(),
325        );
326        self.set_path_with_info(path, cyaw, ck);
327
328        // Initialize state
329        let mut state = VehicleState::new(
330            waypoints[0].0,
331            waypoints[0].1,
332            self.path_yaw.first().copied().unwrap_or(0.0),
333            0.0,
334            self.config.wheelbase,
335        );
336
337        let mut trajectory = vec![(state.x, state.y)];
338        let dt = 0.1;
339        let t_max = 100.0;
340        let mut time = 0.0;
341
342        while time < t_max {
343            let ai = self.compute_acceleration(target_speed, state.v);
344            let di = self.compute_steering(&state);
345            state.update(ai, di, dt);
346            time += dt;
347
348            trajectory.push((state.x, state.y));
349
350            if self.is_goal_reached_vehicle(&state) {
351                break;
352            }
353        }
354
355        Some(trajectory)
356    }
357}
358
359impl PathTracker for RearWheelFeedbackController {
360    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
361        // Adopt a new reference path (comparing contents, not just length)
362        if self.path != *path {
363            self.set_path(path.clone());
364        }
365
366        let vehicle_state = VehicleState::new(
367            current_state.x,
368            current_state.y,
369            current_state.yaw,
370            current_state.v,
371            self.config.wheelbase,
372        );
373
374        let delta = self.compute_steering(&vehicle_state);
375
376        // Compute speed control (assume constant target speed)
377        let target_speed = 5.0; // m/s
378        let v = current_state.v + self.compute_acceleration(target_speed, current_state.v) * 0.1;
379
380        // Convert steering angle to angular velocity
381        let omega = v * delta.tan() / self.config.wheelbase;
382
383        ControlInput::new(v, omega)
384    }
385
386    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
387        let dx = current_state.x - goal.x;
388        let dy = current_state.y - goal.y;
389        (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
390    }
391}
392
393#[cfg(test)]
394mod tests {
395    use super::*;
396    use std::f64::consts::PI;
397
398    #[test]
399    fn test_rear_wheel_feedback_creation() {
400        let config = RearWheelFeedbackConfig::default();
401        let controller = RearWheelFeedbackController::new(config);
402        assert!(controller.path.is_empty());
403    }
404
405    #[test]
406    fn test_rear_wheel_feedback_config_defaults() {
407        let config = RearWheelFeedbackConfig::default();
408        assert!((config.kth - 1.0).abs() < 1e-10);
409        assert!((config.ke - 0.5).abs() < 1e-10);
410        assert!((config.wheelbase - 2.9).abs() < 1e-10);
411    }
412
413    #[test]
414    fn test_rear_wheel_feedback_set_path() {
415        let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
416        let path = Path2D::from_points(vec![
417            Point2D::new(0.0, 0.0),
418            Point2D::new(1.0, 0.0),
419            Point2D::new(2.0, 0.0),
420        ]);
421        controller.set_path(path);
422        assert_eq!(controller.get_path().len(), 3);
423        assert_eq!(controller.path_yaw.len(), 3);
424        assert_eq!(controller.path_curvature.len(), 3);
425    }
426
427    #[test]
428    fn test_rear_wheel_feedback_normalize_angle() {
429        assert!((normalize_angle(3.0 * PI) - PI).abs() < 0.01);
430        assert!((normalize_angle(-3.0 * PI) + PI).abs() < 0.01);
431        assert!((normalize_angle(0.5)).abs() - 0.5 < 0.01);
432    }
433
434    #[test]
435    fn test_rear_wheel_feedback_steering_straight_path() {
436        let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
437        let path = Path2D::from_points(vec![
438            Point2D::new(0.0, 0.0),
439            Point2D::new(10.0, 0.0),
440            Point2D::new(20.0, 0.0),
441            Point2D::new(30.0, 0.0),
442        ]);
443        controller.set_path(path);
444
445        // Vehicle on path, heading along path
446        let state = VehicleState::new(5.0, 0.0, 0.0, 5.0, 2.9);
447        let steering = controller.compute_steering(&state);
448
449        // Should have near-zero steering for vehicle on path
450        assert!(steering.abs() < 0.1);
451    }
452
453    #[test]
454    fn test_rear_wheel_feedback_steering_lateral_error() {
455        let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
456        let path = Path2D::from_points(vec![
457            Point2D::new(0.0, 0.0),
458            Point2D::new(10.0, 0.0),
459            Point2D::new(20.0, 0.0),
460            Point2D::new(30.0, 0.0),
461        ]);
462        controller.set_path(path);
463
464        let state = VehicleState::new(5.0, 2.0, 0.1, 5.0, 2.9);
465        let steering = controller.compute_steering(&state);
466
467        assert!(
468            steering < 0.0,
469            "Steering should be negative to correct errors, got: {}",
470            steering
471        );
472    }
473
474    #[test]
475    fn test_rear_wheel_feedback_planning() {
476        let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
477        let waypoints = vec![(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)];
478
479        let result = controller.planning(waypoints, 5.0, 0.5);
480        assert!(result.is_some());
481        assert!(!result.unwrap().is_empty());
482    }
483
484    #[test]
485    fn test_rear_wheel_feedback_curved_path() {
486        let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
487        let waypoints = vec![(0.0, 0.0), (10.0, 0.0), (20.0, 5.0), (30.0, 10.0)];
488
489        let result = controller.planning(waypoints, 3.0, 0.5);
490        assert!(result.is_some());
491
492        let trajectory = result.unwrap();
493        assert!(!trajectory.is_empty());
494
495        // Check that trajectory ends near goal
496        let last = trajectory.last().unwrap();
497        let dx = last.0 - 30.0;
498        let dy = last.1 - 10.0;
499        let dist = (dx * dx + dy * dy).sqrt();
500        assert!(dist < 2.0, "Final distance to goal: {}", dist);
501    }
502
503    #[test]
504    fn test_vehicle_state_update() {
505        let mut state = VehicleState::new(0.0, 0.0, 0.0, 1.0, 2.9);
506        state.update(0.0, 0.0, 1.0);
507
508        // Should move forward 1m with no steering
509        assert!((state.x - 1.0).abs() < 0.01);
510        assert!(state.y.abs() < 0.01);
511        assert!(state.yaw.abs() < 0.01);
512    }
513
514    #[test]
515    fn test_path_tracker_trait() {
516        let mut controller = RearWheelFeedbackController::new(RearWheelFeedbackConfig::default());
517        let path = Path2D::from_points(vec![
518            Point2D::new(0.0, 0.0),
519            Point2D::new(10.0, 0.0),
520            Point2D::new(20.0, 0.0),
521        ]);
522
523        let state = State2D::new(5.0, 0.0, 0.0, 5.0);
524        let control = controller.compute_control(&state, &path);
525
526        // Control input should be reasonable
527        assert!(control.v.is_finite());
528        assert!(control.omega.is_finite());
529    }
530}