path_planning/cspace/
euclidean.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3use nalgebra::constraint::{SameNumberOfRows, ShapeConstraint};
4use nalgebra::storage::Storage;
5use nalgebra::{Const, Dim, SVector, Vector};
6use rand::distributions::{uniform::SampleUniform, Distribution};
7use rand::{thread_rng, SeedableRng};
8use serde::{de::DeserializeOwned, Deserialize, Serialize};
9
10use crate::cspace::CSpace;
11use crate::error::{InvalidParamError, Result};
12use crate::params::FromParams;
13use crate::rng::{LinearCoordinates, RNG};
14use crate::scalar::Scalar;
15use crate::trajectories::{EuclideanTrajectory, FullTraj};
16use crate::util::bounds::Bounds;
17
18#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
19#[serde(bound(
20  serialize = "X: Serialize",
21  deserialize = "X: DeserializeOwned"
22))]
23pub struct EuclideanSpaceParams<X: Scalar, const N: usize> {
24  pub bounds: Bounds<X, N>,
25  pub seed: Option<u64>,
26}
27
28/// A uniformly sampled Cuboid with euclidean distance cost function
29pub struct EuclideanSpace<X, const N: usize>
30where
31  X: SampleUniform,
32{
33  volume: X,
34  rng: RNG,
35  distribution: LinearCoordinates<X, N>,
36}
37
38impl<X, const N: usize> EuclideanSpace<X, N>
39where
40  X: Scalar + SampleUniform,
41{
42  pub fn new(bounds: Bounds<X, N>, rng: RNG) -> Result<Self> {
43    if !bounds.is_valid() {
44      Err(InvalidParamError {
45        parameter_name: "bounds",
46        parameter_value: format!("{:?}", bounds),
47      })?;
48    }
49
50    let volume = bounds.volume();
51    let distribution = bounds.into();
52
53    Ok(Self {
54      volume,
55      rng,
56      distribution,
57    })
58  }
59}
60
61impl<X, const N: usize> CSpace<X, N> for EuclideanSpace<X, N>
62where
63  X: Scalar + SampleUniform,
64{
65  type Traj = EuclideanTrajectory<X, N>;
66
67  fn volume(&self) -> X {
68    self.volume
69  }
70
71  fn cost<R1, R2, S1, S2>(
72    &self,
73    a: &Vector<X, R1, S1>,
74    b: &Vector<X, R2, S2>,
75  ) -> X
76  where
77    X: Scalar,
78    R1: Dim,
79    R2: Dim,
80    S1: Storage<X, R1>,
81    S2: Storage<X, R2>,
82    ShapeConstraint: SameNumberOfRows<R1, R2>
83      + SameNumberOfRows<R1, Const<N>>
84      + SameNumberOfRows<R2, Const<N>>,
85  {
86    a.metric_distance(b)
87  }
88
89  fn trajectory<S1, S2>(
90    &self,
91    start: Vector<X, Const<N>, S1>,
92    end: Vector<X, Const<N>, S2>,
93  ) -> Option<FullTraj<X, Self::Traj, S1, S2, N>>
94  where
95    X: Scalar,
96    S1: Storage<X, Const<N>>,
97    S2: Storage<X, Const<N>>,
98  {
99    // Assumes that points are only sampled and saturated in this space
100    Some(FullTraj::new(start, end, EuclideanTrajectory::new()))
101  }
102
103  fn is_free<S>(&self, _: &Vector<X, Const<N>, S>) -> bool
104  where
105    S: Storage<X, Const<N>>,
106  {
107    // Assumes that points are only sampled and saturated in this space
108    true
109  }
110
111  fn saturate(&self, a: &mut SVector<X, N>, b: &SVector<X, N>, delta: X) {
112    let scale = delta / a.metric_distance(b);
113    *a -= *b;
114    *a *= scale;
115    *a += *b;
116  }
117
118  fn sample(&mut self) -> SVector<X, N> {
119    self.distribution.sample(&mut self.rng)
120  }
121}
122
123impl<X, const N: usize> FromParams for EuclideanSpace<X, N>
124where
125  X: Scalar + SampleUniform,
126{
127  type Params = EuclideanSpaceParams<X, N>;
128  fn from_params(params: Self::Params) -> Result<Self> {
129    let rng = match params.seed {
130      Some(seed) => RNG::seed_from_u64(seed),
131      None => RNG::from_rng(thread_rng())?,
132    };
133    EuclideanSpace::new(params.bounds, rng)
134  }
135}
136
137#[cfg(test)]
138mod tests {
139
140  use rand::SeedableRng;
141  use rayon::iter::{IntoParallelIterator, ParallelIterator};
142
143  use super::*;
144
145  const SEED: u64 = 0xe580e2e93fd6b040;
146
147  #[test]
148  fn test_parallel_sample() {
149    let mins: [f32; 2] = [-2.0, -2.0];
150    let maxs = [2.0, 2.0];
151    let rng = RNG::seed_from_u64(SEED);
152
153    let bounds = Bounds::new(mins.into(), maxs.into());
154    let space = EuclideanSpace::new(bounds, rng).unwrap();
155
156    let samples = (0..1000)
157      .into_par_iter()
158      .map(|_| {
159        let point = [-1.0, -1.0].into();
160        space.is_free(&point) // parallel read access to config space
161      })
162      .collect::<Vec<_>>();
163
164    assert_eq!(samples.len(), 1000);
165    let point = [-1.0, -1.0].into();
166    assert_eq!(space.is_free(&point), true);
167  }
168}
path_planning/cspace/euclidean.rs

path_planning/cspace/
euclidean.rs
