path_planning/trajectories/
mod.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3mod euclidean;
4mod linear;
5
6pub use euclidean::*;
7pub use linear::*;
8
9////////////////////////////////////////////////////////////////////////////////
10
11use nalgebra::storage::Storage;
12use nalgebra::{ArrayStorage, Const, SVector, Scalar, Vector};
13use serde::{de::DeserializeOwned, Deserialize, Serialize};
14
15/// Genericly stored trajectory, implemented by [`FullTraj`] and [`FullTrajRef`]
16///
17/// Generic parameter S details what kind of nalgebra storage is being used
18pub trait FullTrajectory<X, T, S1, S2, const N: usize>
19where
20  T: Trajectory<X, N>,
21  S1: Storage<X, Const<N>>,
22  S2: Storage<X, Const<N>>,
23{
24  /// The starting coordinate of the trajectory
25  fn start(&self) -> &Vector<X, Const<N>, S1>;
26
27  /// The ending coordinate of the trajectory
28  fn end(&self) -> &Vector<X, Const<N>, S2>;
29
30  /// The extra [`Trajectory`]
31  fn trajectory(&self) -> &T;
32
33  /// The cost to move along the trajectory
34  fn cost(&self) -> X;
35
36  /// Interpolate a coordinate between the start and end points
37  ///
38  /// Start point is assumed to be at t = 0.0 and end point at t = 1.0
39  fn interpolate(&self, t: X) -> SVector<X, N>;
40}
41
42/// A trajectory with array storage
43pub type FullTrajOwned<X, T, const N: usize> =
44  FullTraj<X, T, ArrayStorage<X, N, 1>, ArrayStorage<X, N, 1>, N>;
45
46/// An owned trajectory
47#[derive(Debug, Clone, Serialize, Deserialize)]
48#[serde(bound(
49  serialize = "X: Serialize, T: Serialize, S1: Serialize, S2: Serialize",
50  deserialize = "X: DeserializeOwned, T: DeserializeOwned, S1: DeserializeOwned, S2: DeserializeOwned"
51))]
52pub struct FullTraj<X, T, S1, S2, const N: usize>
53where
54  X: Scalar,
55  T: Trajectory<X, N>,
56  S1: Storage<X, Const<N>>,
57  S2: Storage<X, Const<N>>,
58{
59  start: Vector<X, Const<N>, S1>,
60  end: Vector<X, Const<N>, S2>,
61  trajectory_data: T,
62}
63
64impl<X, T, S1, S2, const N: usize> FullTraj<X, T, S1, S2, N>
65where
66  X: Scalar,
67  T: Trajectory<X, N>,
68  S1: Storage<X, Const<N>>,
69  S2: Storage<X, Const<N>>,
70{
71  pub fn new(
72    start: Vector<X, Const<N>, S1>,
73    end: Vector<X, Const<N>, S2>,
74    trajectory_data: T,
75  ) -> Self {
76    Self {
77      start,
78      end,
79      trajectory_data,
80    }
81  }
82
83  pub fn get_ref<'a>(&'a self) -> FullTrajRef<'a, X, T, S1, S2, N> {
84    FullTrajRef::new(&self.start, &self.end, &self.trajectory_data)
85  }
86
87  pub fn to_trajectory(self) -> T {
88    self.trajectory_data
89  }
90}
91
92impl<X, T, S1, S2, const N: usize> FullTrajectory<X, T, S1, S2, N>
93  for FullTraj<X, T, S1, S2, N>
94where
95  X: Scalar,
96  T: Trajectory<X, N>,
97  S1: Storage<X, Const<N>>,
98  S2: Storage<X, Const<N>>,
99{
100  fn start(&self) -> &Vector<X, Const<N>, S1> {
101    &self.start
102  }
103
104  fn end(&self) -> &Vector<X, Const<N>, S2> {
105    &self.end
106  }
107
108  fn trajectory(&self) -> &T {
109    &self.trajectory_data
110  }
111
112  fn cost(&self) -> X {
113    match self.trajectory_data.cost() {
114      Some(cost) => cost,
115      None => self.trajectory_data.calc_cost(&self.start, &self.end),
116    }
117  }
118
119  fn interpolate(&self, t: X) -> SVector<X, N> {
120    self.trajectory_data.interpolate(&self.start, &self.end, t)
121  }
122}
123
124/// A reference trajectory to an trajectory with array storage
125pub type FullTrajRefOwned<'a, X, T, const N: usize> =
126  FullTrajRef<'a, X, T, ArrayStorage<X, N, 1>, ArrayStorage<X, N, 1>, N>;
127
128/// A
129#[derive(Debug, Clone)]
130pub struct FullTrajRef<'a, X, T, S1, S2, const N: usize>
131where
132  T: Trajectory<X, N>,
133  S1: Storage<X, Const<N>>,
134  S2: Storage<X, Const<N>>,
135{
136  start: &'a Vector<X, Const<N>, S1>,
137  end: &'a Vector<X, Const<N>, S2>,
138  trajectory_data: &'a T,
139}
140
141impl<'a, X, T, S1, S2, const N: usize> FullTrajRef<'a, X, T, S1, S2, N>
142where
143  X: Scalar,
144  T: Trajectory<X, N>,
145  S1: Storage<X, Const<N>>,
146  S2: Storage<X, Const<N>>,
147{
148  pub fn new(
149    start: &'a Vector<X, Const<N>, S1>,
150    end: &'a Vector<X, Const<N>, S2>,
151    trajectory_data: &'a T,
152  ) -> Self {
153    Self {
154      start,
155      end,
156      trajectory_data,
157    }
158  }
159}
160
161impl<'a, X, T, S1, S2, const N: usize> FullTrajRef<'a, X, T, S1, S2, N>
162where
163  X: Scalar,
164  T: Trajectory<X, N> + Clone,
165  S1: Storage<X, Const<N>> + Clone,
166  S2: Storage<X, Const<N>> + Clone,
167{
168  pub fn get_owned(&self) -> FullTraj<X, T, S1, S2, N> {
169    FullTraj::new(
170      self.start.clone(),
171      self.end.clone(),
172      self.trajectory_data.clone(),
173    )
174  }
175}
176
177impl<'a, X, T, S1, S2, const N: usize> FullTrajectory<X, T, S1, S2, N>
178  for FullTrajRef<'a, X, T, S1, S2, N>
179where
180  T: Trajectory<X, N>,
181  S1: Storage<X, Const<N>>,
182  S2: Storage<X, Const<N>>,
183{
184  fn start(&self) -> &Vector<X, Const<N>, S1> {
185    self.start
186  }
187
188  fn end(&self) -> &Vector<X, Const<N>, S2> {
189    self.end
190  }
191
192  fn trajectory(&self) -> &T {
193    self.trajectory_data
194  }
195
196  fn cost(&self) -> X {
197    match self.trajectory_data.cost() {
198      Some(cost) => cost,
199      None => self.trajectory_data.calc_cost(self.start, self.end),
200    }
201  }
202
203  fn interpolate(&self, t: X) -> SVector<X, N> {
204    self.trajectory_data.interpolate(self.start, self.end, t)
205  }
206}
207
208/// The data saved on the edge in the graph need to support the cost and interpolate functions
209pub trait Trajectory<X, const N: usize>: Clone {
210  /// The cached cost to move along the trajectory
211  ///
212  /// Doesn't required endpoints
213  fn cost(&self) -> Option<X>;
214
215  /// Calculate the cost from start to end across this trajectory
216  ///
217  /// Start and end points must the same as the ones used when the original trajectory was created
218  /// to get the sensible output. Implementations are free to panic if start and end points differ.
219  fn calc_cost<S1, S2>(
220    &self,
221    start: &Vector<X, Const<N>, S1>,
222    end: &Vector<X, Const<N>, S2>,
223  ) -> X
224  where
225    S1: Storage<X, Const<N>>,
226    S2: Storage<X, Const<N>>;
227
228  /// Interpolate a coordinate between the start and end points
229  ///
230  /// Start point is assumed to be at t = 0.0 and end point at t = 1.0
231  ///
232  /// Start and end points must the same as the ones used when the original trajectory was created
233  /// to get the sensible output. Implementations are free to panic if start and end points differ.
234  fn interpolate<S1, S2>(
235    &self,
236    start: &Vector<X, Const<N>, S1>,
237    end: &Vector<X, Const<N>, S2>,
238    t: X,
239  ) -> SVector<X, N>
240  where
241    S1: Storage<X, Const<N>>,
242    S2: Storage<X, Const<N>>;
243}
path_planning/trajectories/mod.rs

path_planning/trajectories/
mod.rs
