oxmpl 0.2.0

The Open Motion-Planning Library but Oxidised
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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133

// Copyright (c) 2025 Junior Sundar
//
// SPDX-License-Identifier: BSD-3-Clause

use rand::Rng;

pub use crate::base::spaces::{
    real_vector_state_space::RealVectorStateSpace, so2_state_space::SO2StateSpace,
};
use crate::base::{error::StateSamplingError, state::State};

/// Defines a space in which planning can be performed.
///
/// A `StateSpace` represents the manifold where states exist. It defines the properties and
/// operations applicable to that space as a whole, such as how to measure distance, how to
/// interpolate between states, and how to generate new states.
///
/// This trait is generic and can be implemented for various types of spaces, like N-dimensional
/// Euclidean vectors (`RealVectorStateSpace`) or 2D rotations (`SO2StateSpace`). Planners are
/// written to be generic over this trait, allowing them to solve problems in any space that
/// implements these fundamental operations.
///
/// # Examples
///
/// ```
/// use std::f64;
/// use oxmpl::base::state::State;
/// use oxmpl::base::space::StateSpace;
/// use oxmpl::base::error::StateSamplingError;
/// use rand::Rng;
///
/// #[derive(Debug, Clone, PartialEq)]
/// struct Point1D {
///     x: f64,
/// }
/// impl State for Point1D {}
///
/// struct LineSegmentSpace {
///     bounds: (f64, f64),
/// }
///
/// impl StateSpace for LineSegmentSpace {
///     type StateType = Point1D;
///
///     fn distance(&self, state1: &Self::StateType, state2: &Self::StateType) -> f64 {
///         (state1.x - state2.x).abs()
///     }
///
///     fn interpolate(&self, from: &Self::StateType, to: &Self::StateType, t: f64, state: &mut Self::StateType) {
///         state.x = from.x + (to.x - from.x) * t;
///     }
///
///     fn enforce_bounds(&self, state: &mut Self::StateType) {
///         state.x = state.x.clamp(self.bounds.0, self.bounds.1);
///     }
///
///     fn satisfies_bounds(&self, state: &Self::StateType) -> bool {
///         state.x >= self.bounds.0 && state.x <= self.bounds.1
///     }
///
///     fn sample_uniform(&self, rng: &mut impl Rng) -> Result<Self::StateType, StateSamplingError> {
///         Ok(Point1D { x: rng.gen_range(self.bounds.0..self.bounds.1) })
///     }
/// }
///
/// let space = LineSegmentSpace { bounds: (0.0, 10.0) };
/// let mut rng = rand::thread_rng();
/// let random_state = space.sample_uniform(&mut rng).unwrap();
///
/// assert!(space.satisfies_bounds(&random_state));
/// ```
pub trait StateSpace {
    /// StateType defines what is acceptable in current StateSpace
    type StateType: State;

    /// Find distance between current state1 and target state2.
    ///
    /// The distance metric is specific to the topology of the space. For example, a
    /// `RealVectorStateSpace` would use Euclidean distance, while an `SO2StateSpace` would compute
    /// the shortest angle on a circle.
    ///
    /// # Parameters
    /// * `state1` - The first state.
    /// * `state2` - The second state.
    fn distance(&self, state1: &Self::StateType, state2: &Self::StateType) -> f64;

    /// Find state interpolated between `from` and `to` states given 0<=`t`<=1.
    ///
    /// The resulting state is a point on the path between `from` and `to`, determined by the
    /// interpolation parameter `t`. The path is assumed to be a straight line.
    ///
    /// # Parameters
    /// * `from` - The starting state for interpolation.
    /// * `to` - The ending state for interpolation.
    /// * `t` - The interpolation factor.
    /// * `state` - A mutable reference to a state that will be updated with the result.
    fn interpolate(
        &self,
        from: &Self::StateType,
        to: &Self::StateType,
        t: f64,
        state: &mut Self::StateType,
    );

    /// Modifies the given state to ensure it conforms to the space's defined bounds.
    ///
    /// A `RealVectorStateSpace` might clamp the values of the state to its min/max bounds.
    ///
    /// While an `SO2StateSpace` would normalise an angle to range (e.g., `[-PI, PI)`). This method
    /// modifies the state in-place.
    fn enforce_bounds(&self, state: &mut Self::StateType);

    /// Checks if a state is within the valid bounds of this space.
    ///
    /// This method only checks against the fundamental boundaries of the space definition. It does
    /// *not* check for things like collisions, which is the job of a `StateValidityChecker`.
    ///
    /// # Returns
    /// Returns `true` if the state is within bounds, `false` otherwise.
    fn satisfies_bounds(&self, state: &Self::StateType) -> bool;

    /// Generates a state uniformly at random from the entire state space.
    ///
    /// This method relies on the space having well-defined, finite bounds.
    ///
    /// # Parameters
    /// * `rng` - A mutable reference to a random number generator.
    ///
    /// # Errors
    /// Returns a `StateSamplingError::UnboundedDimension` if the space is unbounded
    /// in any dimension, as uniform sampling from an infinite domain is not possible.
    fn sample_uniform(&self, rng: &mut impl Rng) -> Result<Self::StateType, StateSamplingError>;
}