rust_robotics_control/
move_to_pose.rs

1//
2// Move to specified pose
3// Author: Daniel Ingram (daniel-s-ingram)
4//         Atsushi Sakai(@Atsushi_twi)
5//         Ryohei Sasaki(@rsasaki0109)
6// P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
7#[cfg(feature = "viz")]
8use gnuplot::{AxesCommon, Caption, Color, Figure};
9use rust_robotics_core::{ControlInput, Path2D, Point2D, Pose2D};
10use std::f64::consts::PI;
11
12#[derive(Debug, Clone, Copy)]
13pub struct MoveToPoseConfig {
14    pub kp_rho: f64,
15    pub kp_alpha: f64,
16    pub kp_beta: f64,
17    pub dt: f64,
18    pub goal_tolerance: f64,
19    pub yaw_tolerance: f64,
20    pub max_steps: usize,
21}
22
23impl Default for MoveToPoseConfig {
24    fn default() -> Self {
25        Self {
26            kp_rho: 9.0,
27            kp_alpha: 15.0,
28            kp_beta: -3.0,
29            dt: 0.01,
30            goal_tolerance: 0.001,
31            yaw_tolerance: 0.05,
32            max_steps: 10_000,
33        }
34    }
35}
36
37#[derive(Debug, Clone, Copy)]
38pub struct MoveToPoseStep {
39    pub pose: Pose2D,
40    pub control: ControlInput,
41    pub distance_to_goal: f64,
42}
43
44#[derive(Debug, Clone)]
45pub struct MoveToPoseResult {
46    pub steps: Vec<MoveToPoseStep>,
47    pub converged: bool,
48}
49
50impl MoveToPoseResult {
51    pub fn final_pose(&self) -> Pose2D {
52        self.steps
53            .last()
54            .map(|step| step.pose)
55            .unwrap_or_else(Pose2D::origin)
56    }
57
58    pub fn iterations(&self) -> usize {
59        self.steps.len().saturating_sub(1)
60    }
61
62    pub fn path(&self) -> Path2D {
63        Path2D::from_points(
64            self.steps
65                .iter()
66                .map(|step| Point2D::new(step.pose.x, step.pose.y))
67                .collect(),
68        )
69    }
70}
71
72pub struct MoveToPoseController {
73    config: MoveToPoseConfig,
74}
75
76impl MoveToPoseController {
77    pub fn new(config: MoveToPoseConfig) -> Self {
78        Self { config }
79    }
80
81    pub fn config(&self) -> MoveToPoseConfig {
82        self.config
83    }
84
85    pub fn simulate(&self, start: Pose2D, goal: Pose2D) -> MoveToPoseResult {
86        let mut pose = start;
87        let mut steps = vec![MoveToPoseStep {
88            pose,
89            control: ControlInput::zero(),
90            distance_to_goal: distance_to_goal(pose, goal),
91        }];
92
93        for _ in 0..self.config.max_steps {
94            let distance = distance_to_goal(pose, goal);
95            let yaw_error = normalize_angle(goal.yaw - pose.yaw).abs();
96            if distance <= self.config.goal_tolerance && yaw_error <= self.config.yaw_tolerance {
97                return MoveToPoseResult {
98                    steps,
99                    converged: true,
100                };
101            }
102
103            let control = self.compute_control(pose, goal);
104            pose = integrate_pose(pose, control, self.config.dt);
105            steps.push(MoveToPoseStep {
106                pose,
107                control,
108                distance_to_goal: distance_to_goal(pose, goal),
109            });
110        }
111
112        MoveToPoseResult {
113            steps,
114            converged: false,
115        }
116    }
117
118    pub fn planning(&self, start: (f64, f64, f64), goal: (f64, f64, f64)) -> Vec<(f64, f64)> {
119        self.simulate(
120            Pose2D::new(start.0, start.1, start.2),
121            Pose2D::new(goal.0, goal.1, goal.2),
122        )
123        .path()
124        .points
125        .into_iter()
126        .map(|point| (point.x, point.y))
127        .collect()
128    }
129
130    fn compute_control(&self, pose: Pose2D, goal: Pose2D) -> ControlInput {
131        let x_diff = goal.x - pose.x;
132        let y_diff = goal.y - pose.y;
133        let rho = (x_diff.powi(2) + y_diff.powi(2)).sqrt();
134        let alpha = normalize_angle(y_diff.atan2(x_diff) - pose.yaw);
135        let beta = normalize_angle(goal.yaw - pose.yaw - alpha);
136
137        let mut v = self.config.kp_rho * rho;
138        if !(-PI / 2.0..=PI / 2.0).contains(&alpha) {
139            v = -v;
140        }
141
142        let omega = self.config.kp_alpha * alpha + self.config.kp_beta * beta;
143        ControlInput::new(v, omega)
144    }
145}
146
147pub fn move_to_pose(start: Pose2D, goal: Pose2D, config: MoveToPoseConfig) -> MoveToPoseResult {
148    MoveToPoseController::new(config).simulate(start, goal)
149}
150
151pub fn normalize_angle(mut angle: f64) -> f64 {
152    while angle > PI {
153        angle -= 2.0 * PI;
154    }
155    while angle < -PI {
156        angle += 2.0 * PI;
157    }
158    angle
159}
160
161fn integrate_pose(pose: Pose2D, control: ControlInput, dt: f64) -> Pose2D {
162    let yaw = normalize_angle(pose.yaw + control.omega * dt);
163    Pose2D::new(
164        pose.x + control.v * yaw.cos() * dt,
165        pose.y + control.v * yaw.sin() * dt,
166        yaw,
167    )
168}
169
170fn distance_to_goal(pose: Pose2D, goal: Pose2D) -> f64 {
171    ((goal.x - pose.x).powi(2) + (goal.y - pose.y).powi(2)).sqrt()
172}
173
174#[cfg(feature = "viz")]
175pub fn demo_move_to_pose() {
176    std::fs::create_dir_all("img/path_tracking").unwrap_or_default();
177
178    let start = Pose2D::origin();
179    let goal = Pose2D::new(10.0, 10.0, -1.5 * PI);
180    let result = move_to_pose(start, goal, MoveToPoseConfig::default());
181    let path = result.path();
182
183    println!("Starting Move to Pose...");
184    println!(
185        "Converged: {}, iterations: {}, final pose: ({:.3}, {:.3}, {:.3})",
186        result.converged,
187        result.iterations(),
188        result.final_pose().x,
189        result.final_pose().y,
190        result.final_pose().yaw
191    );
192
193    let mut fg = Figure::new();
194    {
195        let axes = fg
196            .axes2d()
197            .set_title("Move to Pose", &[])
198            .set_x_label("x [m]", &[])
199            .set_y_label("y [m]", &[])
200            .set_aspect_ratio(gnuplot::AutoOption::Fix(1.0));
201
202        let traj_x = path.x_coords();
203        let traj_y = path.y_coords();
204        axes.lines(&traj_x, &traj_y, &[Caption("Trajectory"), Color("green")]);
205        axes.points([start.x], [start.y], &[Caption("Start"), Color("blue")]);
206        axes.points([goal.x], [goal.y], &[Caption("Goal"), Color("red")]);
207
208        let arrow_start_x = vec![start.x, start.x + start.yaw.cos()];
209        let arrow_start_y = vec![start.y, start.y + start.yaw.sin()];
210        axes.lines(
211            &arrow_start_x,
212            &arrow_start_y,
213            &[Caption("Start Orientation"), Color("blue")],
214        );
215
216        let arrow_goal_x = vec![goal.x, goal.x + goal.yaw.cos()];
217        let arrow_goal_y = vec![goal.y, goal.y + goal.yaw.sin()];
218        axes.lines(
219            &arrow_goal_x,
220            &arrow_goal_y,
221            &[Caption("Goal Orientation"), Color("red")],
222        );
223    }
224
225    let output_path = "img/path_tracking/move_to_pose.png";
226    fg.set_terminal("pngcairo", output_path);
227    fg.show()
228        .expect("failed to render gnuplot figure for move_to_pose");
229    println!("Move to pose visualization saved to: {}", output_path);
230    println!("Move to Pose complete!");
231}
232
233#[cfg(test)]
234mod tests {
235    use super::*;
236
237    #[test]
238    fn test_normalize_angle_wraps_to_pi_range() {
239        let wrapped = normalize_angle(3.5 * PI);
240        assert!((-PI..=PI).contains(&wrapped));
241    }
242
243    #[test]
244    fn test_move_to_pose_converges() {
245        let start = Pose2D::origin();
246        let goal = Pose2D::new(5.0, 5.0, -PI / 2.0);
247
248        let result = move_to_pose(start, goal, MoveToPoseConfig::default());
249        let final_pose = result.final_pose();
250
251        assert!(result.converged);
252        assert!((final_pose.x - goal.x).abs() < 0.05);
253        assert!((final_pose.y - goal.y).abs() < 0.05);
254        assert!(normalize_angle(final_pose.yaw - goal.yaw).abs() < 0.05);
255    }
256
257    #[test]
258    fn test_move_to_pose_returns_non_empty_path() {
259        let controller = MoveToPoseController::new(MoveToPoseConfig::default());
260        let path = controller.planning((0.0, 0.0, 0.0), (2.0, 1.0, 0.0));
261
262        assert!(path.len() > 1);
263        assert_eq!(path.first().copied(), Some((0.0, 0.0)));
264    }
265}
rust_robotics_control/move_to_pose.rs

rust_robotics_control/
move_to_pose.rs
