Class VanaStateSpace

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class VanaStateSpace : public ompl::base::CompoundStateSpace

An R^4 x SO(2) state space where distance is measured by the length of a type 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] P. Váňa, A. Alves Neto, J. Faigl, and D. G. Macharet, “Minimal 3D Dubins Path with Bounded Curvature and Pitch Angle,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), May 2020, pp. 8497–8503. doi: 10.1109/ICRA40945.2020.9197084.

Public Functions

VanaStateSpace(double turningRadius = 1.0, double maxPitch = boost::math::double_constants::sixth_pi)
VanaStateSpace(double turningRadius, std::pair<double, double> pitchRange)
~VanaStateSpace() 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.

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

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

Parameters:

  • from – start state

  • path – a segment

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

  • state – the state that lies on the segment at fraction t of the distance along path, starting from state from.

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

Compute a 3D Dubins path using the model and algorithm proposed by Vana 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 State *state1, const State *state2, double radius, PathType &path, DubinsStateSpace::StateType *endSZ) 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

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)

Public Members

double horizontalRadius_ = {1.}
double verticalRadius_ = {1.}
DubinsStateSpace::PathType pathXY_
DubinsStateSpace::PathType pathSZ_
DubinsStateSpace::StateType *startSZ_ = {nullptr}

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()