Class AtlasStateSpace

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Derived Type

Class Documentation

class AtlasStateSpace : public ompl::base::ConstrainedStateSpace

ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint manifold.

L. Jaillet and J. M. Porta, “Path Planning Under Kinematic Constraints by Rapidly Exploring Manifolds,” in IEEE Transactions on Robotics, vol. 29, no. 1, pp. 105-117, Feb. 2013. DOI: 10.1109/TRO.2012.2222272.

Short Description

AtlasStateSpace implements an atlas-based methodology for constrained sampling-based planning, where the underlying constraint manifold is locally parameterized by charts (AtlasChart). The underlying constraint manifold can then be sampled and explored using the collection of these charts (an atlas).

External Documentation

This state space is inspired by the work on AtlasRRT. One reference of which is the following:

For more information on constrained sampling-based planning using atlas-based methods, see the following review paper. The section on atlas-based methods cites most of the relevant literature.

Z. Kingston, M. Moll, and L. E. Kavraki, “Sampling-Based Methods for Motion Planning with Constraints,” Annual Review of Control, Robotics, and Autonomous Systems, 2018. DOI: 10.1146/annurev-control-060117-105226 [PDF].

Subclassed by ompl::base::TangentBundleStateSpace

Tunable Parameters

double epsilon_ = {ompl::magic::ATLAS_STATE_SPACE_EPSILON}

Maximum distance between a chart and the manifold inside its validity region.

double rho_

Maximum radius of chart validity region.

double cos_alpha_

Cosine of the maximum angle between a chart and the manifold inside its validity region.

double exploration_

Balance between explorationa and refinement.

mutable double rho_s_

Sampling radius within a chart. Inferred from rho and exploration parameters.

double backoff_ = {ompl::magic::ATLAS_STATE_SPACE_BACKOFF}

Step size reduction factor during manifold traversal.

unsigned int maxChartsPerExtension_ = {ompl::magic::ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION}

Maximum number of charts that can be created in one manifold traversal.

Getters and Setters

inline void setEpsilon(double epsilon)

Set epsilon, the maximum permissible distance between a point in the validity region of a chart and its projection onto the manifold. Default 0.1.

inline void setRho(double rho)

Set rho, the maximum radius for which a chart is valid. Default 0.1.

inline void setAlpha(double alpha)

Set alpha, the maximum permissible angle between the chart and the manifold inside the validity region of the chart. Must be within the range (0, pi/2). Default pi/16.

inline void setExploration(double exploration)

Set the exploration parameter, which tunes the balance of refinement (sampling within known regions) and exploration (sampling on the frontier). Valid values are in the range [0,1), where 0 is all refinement, and 1 is all exploration. Default 0.5.

inline void setMaxChartsPerExtension(unsigned int charts)

Sometimes manifold traversal creates many charts. This parameter limits the number of charts that can be created during one traversal. Default 200.

inline void setBiasFunction(const AtlasChartBiasFunction &biasFunction)

Sets bias function for sampling.

inline void setSeparated(bool separate)

Sets whether the atlas should separate charts or not.

inline void setBackoff(double backoff)

Sets the backoff factor in manifold traversal factor.

inline double getEpsilon() const

Get epsilon.

inline double getRho() const

Get rho.

inline double getAlpha() const

Get alpha.

inline double getExploration() const

Get the exploration parameter.

inline double getRho_s() const

Get the sampling radius.

inline unsigned int getMaxChartsPerExtension() const

Get the maximum number of charts to create in one pass.

inline bool isSeparated() const

Returns whether the atlas is separating charts or not.

inline std::size_t getChartCount() const

Return the number of charts currently in the atlas.

inline double getBackoff() const

Returns the current backoff factor used in manifold traversal.

Atlas Chart Management

AtlasChart *newChart(const StateType *state) const

Create a new chart for the atlas, centered at xorigin, which should be on the manifold. Returns nullptr upon failure.

AtlasChart *sampleChart() const

Pick a chart at random.

AtlasChart *owningChart(const StateType *state) const

Find the chart to which x belongs. Returns nullptr if no chart found. Assumes x is already on the manifold.

AtlasChart *getChart(const StateType *state, bool force = false, bool *created = nullptr) const

Wrapper to return chart state belongs to. Will attempt to initialize new chart if state does not belong to one. If force is true, this routine will reinitialize the chart that the state should be on. If created is not null, it will be set to true if a new chart is created.

Constrained Planning

AtlasChart *anchorChart(const State *state) const

Wrapper for newChart(). Charts created this way will persist through calls to clear().

Throws:

ompl::Exception – if manifold seems degenerate here.

virtual bool discreteGeodesic(const State *from, const State *to, bool interpolate = false, std::vector<State*> *geodesic = nullptr) const override

Traverse the manifold from from toward to. Returns true if we reached to, and false if we stopped early for any reason, such as a collision or traveling too far. No collision checking is performed if interpolate is true. If geodesic is not nullptr, the sequence of intermediates is saved to it, including a copy of from, as well as the final state, which is a copy of to if we reached to. Caller is responsible for freeing states returned in geodesic.

Statistics and Visualization

double estimateFrontierPercent() const

Estimate what percentage of atlas charts do not have fully formed polytope boundaries, and are therefore on the frontier.

void printPLY(std::ostream &out) const

Write a mesh representation of the atlas to a stream.

Public Types

using AtlasChartBiasFunction = std::function<double(AtlasChart*)>
using NNElement = std::pair<const StateType*, std::size_t>

Public Functions

AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate = true)

Construct an atlas with the specified dimensions.

~AtlasStateSpace() override

Destructor.

virtual void clear() override

Reset the space (except for anchor charts).

inline virtual StateSamplerPtr allocDefaultStateSampler() const override

Allocate the default state sampler for this space.

inline virtual StateSamplerPtr allocStateSampler() const override

Allocate the previously set state sampler for this space.

inline virtual void copyState(State *destination, const State *source) const override

Copy a state to another. The memory of source and destination should NOT overlap.

Note

For more advanced state copying methods (partial copy, for example), see Advanced methods for copying states.

inline virtual State *allocState() const override

Allocate a new state in this space.

Protected Attributes

mutable std::vector<StateType*> anchors_

Set of states on which there are anchored charts.

mutable std::vector<AtlasChart*> charts_

Set of charts.

mutable PDF<AtlasChart*> chartPDF_

PDF of charts according to a bias function.

mutable NearestNeighborsGNAT<NNElement> chartNN_

Set of chart centers and indices, accessible by nearest-neighbor queries to the chart centers.

AtlasChartBiasFunction biasFunction_

Function to bias chart sampling.

bool separate_

Enable or disable halfspace separation of the charts.

mutable RNG rng_

Random number generator.

class StateType : public ompl::base::ConstrainedStateSpace::StateType

A state in an atlas represented as a real vector in ambient space and a chart that it belongs to.

Public Functions

inline StateType(const ConstrainedStateSpace *space)

Construct state of size n.

inline AtlasChart *getChart() const

Get the chart this state is on.

inline void setChart(AtlasChart *c) const

Set the chart c for the state.