path_planning/
path_planner.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3use std::marker::PhantomData;
4
5use nalgebra::SVector;
6use num_traits::Zero;
7use serde::{de::DeserializeOwned, Deserialize, Serialize};
8
9use crate::cspace::CSpace;
10use crate::error::Result;
11use crate::obstacles::ObstacleSpace;
12use crate::params::FromParams;
13use crate::run::Runnable;
14use crate::scalar::Scalar;
15use crate::trajectories::{FullTrajOwned, FullTrajRefOwned, FullTrajectory};
16
17/// A local goal location, specifies whether this location is new or if it is the same as before.
18#[derive(Clone, Copy, Debug)]
19pub enum MoveGoal<X, const N: usize> {
20  /// The current move goal has changed
21  New(SVector<X, N>),
22  /// The current move goal has not changed
23  Old(SVector<X, N>),
24  /// The global goal has been reached
25  Finished,
26}
27
28/// Information about the robot sizing
29#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
30#[serde(bound(
31  serialize = "X: Serialize,",
32  deserialize = "X: DeserializeOwned,",
33))]
34pub struct RobotSpecs<X: Scalar> {
35  /// The radius of the bounding sphere around the robot
36  pub robot_radius: X,
37  /// The radius around the robot that is able to sense obstacles
38  pub sensor_radius: X,
39}
40
41/// A sampling-based path path_planner
42pub trait PathPlanner<X, CS, OS, const N: usize>: Sized
43where
44  X: Scalar,
45  CS: CSpace<X, N>,
46  OS: ObstacleSpace<X, CS, N>,
47{
48  type Params: Clone;
49  type State: Clone;
50
51  /// Define a new Path Planner with an initial set of obstacles in the space.
52  ///
53  /// `init`: The inital pose, must be in free space.
54  ///
55  /// `goal`: The goal pose, must be in free space, the root of the search tree.
56  ///
57  /// `robot_specs`: Information about the robot sizing.
58  ///
59  /// `cspace`: Describes the constant configuration space to plan within.
60  ///
61  /// `obs_space`: The obstacle space to plan around.
62  ///
63  /// `params`: Algorithm specific parameters.
64  fn new(
65    init: SVector<X, N>,
66    goal: SVector<X, N>,
67    robot_specs: RobotSpecs<X>,
68    cspace: CS,
69    obs_space: OS,
70    params: Self::Params,
71  ) -> Result<Self>;
72
73  /// Samples nodes until one is added to the search graph.
74  ///
75  /// Returns updated state reference once node is successfully added to the graph
76  fn create_node(&mut self) -> &Self::State;
77
78  /// Sample once to add a node to the graph
79  ///
80  /// Optionally returns updated state if node was successfully added
81  fn sample_node(&mut self) -> Option<&Self::State>;
82
83  /// Checks the sensor on the robot for obstacles updates within robot_specs.sensor_range
84  fn check_sensors(&mut self);
85
86  /// Returns reference to the currently used obstacle space
87  fn get_obs_space(&self) -> &OS;
88
89  /// Returns mutable reference to the currently used obstacle space
90  ///
91  /// Make sure to call [`Self::check_nodes`] with `obs_space = true` if the modifications
92  /// might make some nodes invalid
93  fn get_obs_space_mut(&mut self) -> &mut OS;
94
95  /// Returns reference to the currently used cspace
96  fn get_cspace(&self) -> &CS;
97
98  /// Returns mutable reference to the currently used cspace
99  ///
100  /// Make sure to call [`Self::check_nodes`] with `cspace = true` if the modifications
101  /// might make some nodes invalid
102  fn get_cspace_mut(&mut self) -> &mut CS;
103
104  /// Checks the spaces specified that all nodes are still valid in that space
105  ///
106  /// Make sure to call after modifying the obs_space or cspace such that some
107  /// nodes might be invalid
108  fn check_nodes(&mut self, obs_space: bool, cspace: bool);
109
110  /// Updates the pose of the system, possibly changing the move goal
111  ///
112  /// Returns Some coordinates the next move goal if the global goal is reachable
113  ///
114  /// if `nearest` is true, will select a move goal that is the closest to the given pose
115  ///
116  /// Specifies wether the returned move goal is new or the same as before
117  fn update_pose(
118    &mut self,
119    pose: SVector<X, N>,
120    nearest: bool,
121  ) -> Option<MoveGoal<X, N>>;
122
123  /// Get the search tree as a list of edges
124  fn get_tree(&self) -> Vec<FullTrajRefOwned<X, CS::Traj, N>>;
125
126  /// Returns Some trajectory the next move goal if the global goal is reachable
127  fn get_current_path(&self) -> Option<FullTrajOwned<X, CS::Traj, N>>;
128
129  /// Returns Some path to the goal if the global goal is reachable
130  fn get_path_to_goal(&self) -> Option<Vec<SVector<X, N>>>;
131
132  /// Returns the total cost to reach the goal from the most recent pose given in update_pose
133  fn get_cost_to_goal(&self) -> Option<X> {
134    let iter = self.get_path_to_goal()?;
135
136    let mut cost = X::zero();
137    let mut last_pose = self.get_last_pose().clone();
138
139    for pose in iter {
140      cost += self.get_cspace().cost(&last_pose, &pose);
141      last_pose = pose;
142    }
143
144    Some(cost)
145  }
146
147  fn get_cost_to_move_goal(&self) -> Option<X> {
148    let current_path = self.get_current_path()?;
149    let move_goal = current_path.end();
150    let pose = self.get_last_pose().clone();
151
152    Some(self.get_cspace().cost(&pose, &move_goal))
153  }
154
155  /// Get the last known pose of the system
156  fn get_last_pose(&self) -> &SVector<X, N>;
157
158  /// Get a reference to the current serializable state information
159  fn get_state(&self) -> &Self::State;
160
161  /// Returns the coordinates of the goal node
162  fn get_goal(&self) -> &SVector<X, N>;
163
164  /// Returns the number of nodes in the graph plus the initial node (which is technically never added to the graph)
165  fn count(&self) -> usize;
166}
167
168#[derive(Serialize, Deserialize)]
169#[serde(bound(
170  serialize = "X: Serialize, PP::Params: Serialize, <CS as FromParams>::Params: Serialize, <OS as FromParams>::Params: Serialize",
171  deserialize = "X: DeserializeOwned, PP::Params: DeserializeOwned, <CS as FromParams>::Params: DeserializeOwned, <OS as FromParams>::Params: DeserializeOwned",
172))]
173pub struct PathPlannerParams<X, CS, OS, PP, const N: usize>
174where
175  X: Scalar,
176  CS: CSpace<X, N>,
177  OS: ObstacleSpace<X, CS, N>,
178  PP: PathPlanner<X, CS, OS, N>,
179{
180  pub init: SVector<X, N>,
181  pub goal: SVector<X, N>,
182  pub robot_specs: RobotSpecs<X>,
183  pub nodes_per_step: usize,
184  pub max_translation_per_step: X,
185  pub planner_params: PP::Params,
186  pub cspace: <CS as FromParams>::Params,
187  pub obstacles: <OS as FromParams>::Params,
188}
189
190impl<X, CS, OS, PP, const N: usize> Clone
191  for PathPlannerParams<X, CS, OS, PP, N>
192where
193  X: Scalar,
194  CS: CSpace<X, N>,
195  OS: ObstacleSpace<X, CS, N>,
196  PP: PathPlanner<X, CS, OS, N>,
197{
198  fn clone(&self) -> Self {
199    Self {
200      init: self.init.clone(),
201      goal: self.goal.clone(),
202      robot_specs: self.robot_specs.clone(),
203      nodes_per_step: self.nodes_per_step.clone(),
204      max_translation_per_step: self.max_translation_per_step.clone(),
205      planner_params: self.planner_params.clone(),
206      cspace: self.cspace.clone(),
207      obstacles: self.obstacles.clone(),
208    }
209  }
210}
211
212/// A generic implementation of [`Runnable`] for implementor for [`PathPlanner`]
213pub struct RunPathPlanner<X, CS, OS, PP, const N: usize>
214where
215  X: Scalar,
216  CS: CSpace<X, N>,
217  OS: ObstacleSpace<X, CS, N>,
218  PP: PathPlanner<X, CS, OS, N>,
219{
220  path_planner: PP,
221  nodes_per_step: usize,
222  max_translation_per_step: X,
223  phantom_data: PhantomData<(CS, OS)>,
224}
225
226impl<X, CS, OS, PP, const N: usize> FromParams
227  for RunPathPlanner<X, CS, OS, PP, N>
228where
229  X: Scalar,
230  CS: CSpace<X, N>,
231  OS: ObstacleSpace<X, CS, N>,
232  PP: PathPlanner<X, CS, OS, N>,
233{
234  type Params = PathPlannerParams<X, CS, OS, PP, N>;
235
236  fn from_params(params: Self::Params) -> Result<Self> {
237    let path_planner = PP::new(
238      params.init,
239      params.goal,
240      params.robot_specs,
241      CS::from_params(params.cspace)?,
242      OS::from_params(params.obstacles)?,
243      params.planner_params,
244    )?;
245
246    Ok(Self {
247      path_planner,
248      nodes_per_step: params.nodes_per_step,
249      max_translation_per_step: params.max_translation_per_step,
250      phantom_data: PhantomData,
251    })
252  }
253}
254
255impl<X, CS, OS, PP, const N: usize> Runnable
256  for RunPathPlanner<X, CS, OS, PP, N>
257where
258  X: Scalar,
259  CS: CSpace<X, N>,
260  OS: ObstacleSpace<X, CS, N>,
261  PP: PathPlanner<X, CS, OS, N>,
262  PP::Params: Clone,
263  PP::State: Clone,
264{
265  type State = PP::State;
266
267  /// TODO: Currently assumes that obstacles are static. Appearing obstacles
268  /// should be dealt with here
269  fn step(&mut self) -> Option<&Self::State> {
270    // Sample new nodes
271    let begin_count = self.path_planner.count();
272    for _ in 0..self.nodes_per_step {
273      self.path_planner.create_node();
274      log::debug!("Added node {} to tree", self.path_planner.count());
275    }
276    log::info!(
277      "Added nodes {}-{} to the tree",
278      begin_count,
279      self.path_planner.count()
280    );
281
282    // Update pose to same location to check if any of the newly added node will
283    // give us a valid path
284    let pose = self.path_planner.get_last_pose().clone();
285    if let Some(MoveGoal::Finished) = self.path_planner.update_pose(pose, false)
286    {
287      return None;
288    }
289
290    if let Some(traj) = self.path_planner.get_current_path() {
291      let target = traj.end();
292      let cspace = self.path_planner.get_cspace();
293      let cost = cspace.cost(&pose, target);
294
295      let new_pose = if cost >= self.max_translation_per_step {
296        let direction = *target - pose;
297        let unit_direction =
298          direction / cspace.cost(&SVector::zero(), &direction);
299        let direction = unit_direction * self.max_translation_per_step;
300        let new_pose = pose + direction;
301        new_pose
302      } else {
303        target.clone()
304      };
305
306      if let Some(MoveGoal::Finished) =
307        self.path_planner.update_pose(new_pose, false)
308      {
309        return None;
310      }
311    }
312    Some(self.path_planner.get_state())
313  }
314}
path_planning/path_planner.rs

path_planning/
path_planner.rs
