path_planning/cspace/
mod.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3mod euclidean;
4pub mod leader_follower;
5mod linear;
6
7pub use euclidean::*;
8pub use linear::*;
9
10////////////////////////////////////////////////////////////////////////////////
11
12use nalgebra::constraint::{SameNumberOfRows, ShapeConstraint};
13use nalgebra::storage::Storage;
14use nalgebra::{Const, Dim, SVector, Scalar, SimdRealField, Vector};
15
16use crate::params::FromParams;
17use crate::trajectories::{FullTraj, Trajectory};
18
19/// A generic configuration space for sampling-based problems
20///
21/// Descibes the state space of the system that a path is being planned for
22pub trait CSpace<X, const N: usize>: FromParams + Sized {
23  type Traj: Trajectory<X, N>;
24
25  /// An overestimate of the volume of the space (should remain constant)
26  fn volume(&self) -> X;
27
28  /// Cost function between two arbitrary points within the space
29  /// Always returns a value
30  ///
31  /// Must satisfy the triangle inequality
32  fn cost<R1, R2, S1, S2>(
33    &self,
34    start: &Vector<X, R1, S1>,
35    end: &Vector<X, R2, S2>,
36  ) -> X
37  where
38    X: SimdRealField,
39    R1: Dim,
40    R2: Dim,
41    S1: Storage<X, R1>,
42    S2: Storage<X, R2>,
43    ShapeConstraint: SameNumberOfRows<R1, R2>
44      + SameNumberOfRows<R1, Const<N>>
45      + SameNumberOfRows<R2, Const<N>>;
46
47  /// Calculate the trajectory data between the given start and end points for the system
48  ///
49  /// Returns None if trajectory isn't possible due to system dynamics
50  fn trajectory<S1, S2>(
51    &self,
52    start: Vector<X, Const<N>, S1>,
53    end: Vector<X, Const<N>, S2>,
54  ) -> Option<FullTraj<X, Self::Traj, S1, S2, N>>
55  where
56    X: Scalar,
57    S1: Storage<X, Const<N>>,
58    S2: Storage<X, Const<N>>;
59
60  /// Determines if the given point is a valid state in the space
61  fn is_free<S>(&self, state: &Vector<X, Const<N>, S>) -> bool
62  where
63    S: Storage<X, Const<N>>;
64
65  /// Modify a such that the cost from a -> b is no more than delta away
66  /// while moving a as little as possible
67  fn saturate(&self, a: &mut SVector<X, N>, b: &SVector<X, N>, delta: X);
68
69  /// Sample a point in the configuration space
70  fn sample(&mut self) -> SVector<X, N>;
71}
path_planning/cspace/mod.rs

path_planning/cspace/
mod.rs
