Class VanaOwenStateSpace

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class VanaOwenStateSpace : public ompl::base::CompoundStateSpace

An R^4 x SO(2) state space where distance is measured by the length of a type of Dubins airplane curves.

Note that distance in this space is not a proper distance metric, so nearest neighbor methods that rely on distance() being a metric (such as ompl::NearestNeighborsGNAT) will not always return the true nearest neighbors or get stuck in an infinite loop.

See the following reference for details:

[1] M. Moll, “3D Dubins Paths for Underwater Vehicles,” OCEANS 2024 - Halifax, Halifax, NS, Canada, 2024. doi: 10.1109/OCEANS55160.2024.10754581.

Public Types

enum PathCategory

Values:

enumerator LOW_ALTITUDE
enumerator MEDIUM_ALTITUDE
enumerator HIGH_ALTITUDE
enumerator UNKNOWN

Public Functions

VanaOwenStateSpace(double turningRadius = 1.0, double maxPitch = boost::math::double_constants::sixth_pi)
VanaOwenStateSpace(double turningRadius, std::pair<double, double> pitchRange)
~VanaOwenStateSpace() override = default
inline virtual bool isMetricSpace() const override

Return true if the distance function associated with the space is a metric.

inline virtual bool hasSymmetricDistance() const override

Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.

inline virtual bool hasSymmetricInterpolate() const override

Check if the interpolation function on this state space is symmetric, i.e. interpolate(from, to,

t, state) = interpolate(to, from, 1-t, state). Default implementation returns true.

inline virtual void sanityChecks() const override

Convenience function that allows derived state spaces to choose which checks should pass (see SanityChecks flags) and how strict the checks are. This just calls sanityChecks() with some default arguments.

inline void setBounds(const RealVectorBounds &bounds)

Set the bounds of this state space. This defines the range of the space in which sampling is performed.

inline const RealVectorBounds &getBounds() const

Get the bounds for this state space.

inline void setTolerance(double tolerance)

Set tolerance for convergence on optimal turning radius

inline double getTolerance()

Get tolerance for convergence on optimal turning radius

virtual State *allocState() const override

Allocate a state that can store a point in the described space.

virtual void registerProjections() override

Register the projections for this state space. Usually, this is at least the default projection. These are implicit projections, set by the implementation of the state space. This is called by setup().

virtual double distance(const State *state1, const State *state2) const override

Computes distance between two states. This function satisfies the properties of a metric if isMetricSpace() is true, and its return value will always be between 0 and getMaximumExtent()

virtual unsigned int validSegmentCount(const State *state1, const State *state2) const override

Count how many segments of the “longest valid length” fit on the motion from state1 to state2. This is the max() of the counts returned by contained subspaces.

virtual void interpolate(const State *from, const State *to, double t, State *state) const override

Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state. The memory location of state is not required to be different from the memory of either from or to.

virtual void interpolate(const State *from, const State *to, double t, PathType &path, State *state) const

Compute the state that lies at time t in [0,1] on the segment that connects from state to to state.

Parameters:

  • from – start state

  • to – end state

  • t – fraction of distance along segment between from and to. Has to be between 0 and 1 (boundaries included).

  • path – the segment connecting @from and @to.

  • state – the state that lies on the segment at fraction t of the distance between from and to.

std::optional<PathType> getPath(const State *state1, const State *state2) const

Compute a 3D Dubins path using the model and algorithm proposed by VanaOwen et al.

Parameters:

  • state1 – start state

  • state2 – end state

Returns:

a 3D Dubins path if one was found, std::nullopt_t otherwise

bool decoupled(const StateType *state1, const StateType *state2, double radius, PathType &path, std::array<DubinsStateSpace::StateType*, 3> &scratch) const

Helper function used by getPath.

Parameters:

  • state1 – start state

  • state2 – end state

  • radius – turning radius for the motion in the XY plane

  • path – resulting path

  • endSZ – working memory

Returns:

true iff a feasible solution was found

Protected Functions

DubinsStateSpace::StateType *get2DPose(double x, double y, double yaw) const

Allocate a DubinsSpace state and initialize it.

Parameters:

  • x – initial X coordinate of allocated state

  • y – initial Y coordinate of allocated state

  • yaw – initial yaw coordinate of allocated state

Returns:

new allocated and initialized state

bool isValid(DubinsStateSpace::PathType const &path, StateType const *state) const

Whether a path in SZ space satisfies Dubins path type and pitch constraints.

Returns:

true iff path is of type LRL or RLR and the pitch in state lies in-between min and max pitch

Protected Attributes

double rho_

Turning radius

double minPitch_

Minimum pitch in radians

double maxPitch_

Maximum pitch in radians

double tolerance_ = {1e-8}

Tolerance used to determine convergence when searching for optimal turning radius

DubinsStateSpace dubinsSpace_

Auxiliary space to compute paths in SE(2) slices of state space

class PathType

Public Functions

PathType()
PathType(const PathType &path)
~PathType()
double length() const
PathType &operator=(const PathType &path)
PathCategory category() const

Public Members

DubinsStateSpace::PathType pathXY_
DubinsStateSpace::PathType pathSZ_
DubinsStateSpace::StateType *startSZ_ = {nullptr}
double horizontalRadius_ = {1.}
double verticalRadius_ = {1.}
double deltaZ_ = {0.}
double phi_ = {0.}
unsigned int numTurns_ = {0}

Friends

friend std::ostream &operator<<(std::ostream &os, const PathType &path)
class StateType : public ompl::base::CompoundState

A state in R^4 x SO(2): (x, y, z, pitch, yaw)

Public Functions

StateType() = default
inline double operator[](unsigned index) const
inline double &operator[](unsigned index)
inline double yaw() const
inline double &yaw()
inline double pitch() const
inline double &pitch()