path-planning 0.1.0

Path Planning Algorithms implemented in Rust.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71

/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */

mod euclidean;
pub mod leader_follower;
mod linear;

pub use euclidean::*;
pub use linear::*;

////////////////////////////////////////////////////////////////////////////////

use nalgebra::constraint::{SameNumberOfRows, ShapeConstraint};
use nalgebra::storage::Storage;
use nalgebra::{Const, Dim, SVector, Scalar, SimdRealField, Vector};

use crate::params::FromParams;
use crate::trajectories::{FullTraj, Trajectory};

/// A generic configuration space for sampling-based problems
///
/// Descibes the state space of the system that a path is being planned for
pub trait CSpace<X, const N: usize>: FromParams + Sized {
  type Traj: Trajectory<X, N>;

  /// An overestimate of the volume of the space (should remain constant)
  fn volume(&self) -> X;

  /// Cost function between two arbitrary points within the space
  /// Always returns a value
  ///
  /// Must satisfy the triangle inequality
  fn cost<R1, R2, S1, S2>(
    &self,
    start: &Vector<X, R1, S1>,
    end: &Vector<X, R2, S2>,
  ) -> X
  where
    X: SimdRealField,
    R1: Dim,
    R2: Dim,
    S1: Storage<X, R1>,
    S2: Storage<X, R2>,
    ShapeConstraint: SameNumberOfRows<R1, R2>
      + SameNumberOfRows<R1, Const<N>>
      + SameNumberOfRows<R2, Const<N>>;

  /// Calculate the trajectory data between the given start and end points for the system
  ///
  /// Returns None if trajectory isn't possible due to system dynamics
  fn trajectory<S1, S2>(
    &self,
    start: Vector<X, Const<N>, S1>,
    end: Vector<X, Const<N>, S2>,
  ) -> Option<FullTraj<X, Self::Traj, S1, S2, N>>
  where
    X: Scalar,
    S1: Storage<X, Const<N>>,
    S2: Storage<X, Const<N>>;

  /// Determines if the given point is a valid state in the space
  fn is_free<S>(&self, state: &Vector<X, Const<N>, S>) -> bool
  where
    S: Storage<X, Const<N>>;

  /// Modify a such that the cost from a -> b is no more than delta away
  /// while moving a as little as possible
  fn saturate(&self, a: &mut SVector<X, N>, b: &SVector<X, N>, delta: X);

  /// Sample a point in the configuration space
  fn sample(&mut self) -> SVector<X, N>;
}