AtlasStateSpace.h
161 AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate = true);
346 AtlasChart *getChart(const StateType *state, bool force = false, bool *created = nullptr) const;
434 }
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
AtlasChart * newChart(const StateType *state) const
Create a new chart for the atlas, centered at xorigin, which should be on the manifold....
Definition: AtlasStateSpace.cpp:265
double getBackoff() const
Returns the current backoff factor used in manifold traversal.
Definition: AtlasStateSpace.h:352
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to.
Definition: AtlasStateSpace.h:167
PDF< AtlasChart * > chartPDF_
PDF of charts according to a bias function.
Definition: AtlasStateSpace.h:424
State * allocState() const override
Allocate a new state in this space.
Definition: AtlasStateSpace.h:220
StateSamplerPtr allocStateSampler() const override
Allocate the previously set state sampler for this space.
Definition: AtlasStateSpace.h:208
A shared pointer wrapper for ompl::base::Constraint.
void interpolate(const State *from, const State *to, double t, State *state) const override
Find a state between from and to around time t, where t = 0 is from, and t = 1 is the final state rea...
Definition: ConstrainedStateSpace.cpp:270
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
Definition: ConstrainedStateSpace.cpp:102
unsigned int getMaxChartsPerExtension() const
Get the maximum number of charts to create in one pass.
Definition: AtlasStateSpace.h:334
NearestNeighborsGNAT< NNElement > chartNN_
Set of chart centers and indices, accessible by nearest-neighbor queries to the chart centers.
Definition: AtlasStateSpace.h:428
std::vector< StateType * > anchors_
Set of states on which there are anchored charts.
Definition: AtlasStateSpace.h:418
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state uniformly from the ball with center near and radius distance. Return sample in state.
Definition: AtlasStateSpace.cpp:99
bool separate_
Enable or disable halfspace separation of the charts.
Definition: AtlasStateSpace.h:460
void setBackoff(double backoff)
Sets the backoff factor in manifold traversal factor.
Definition: AtlasStateSpace.h:298
AtlasChart * anchorChart(const State *state) const
Wrapper for newChart(). Charts created this way will persist through calls to clear().
Definition: AtlasStateSpace.cpp:251
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state uniformly from a normal distribution with given mean and stdDev. Return sample in stat...
Definition: AtlasStateSpace.cpp:150
void setBiasFunction(const AtlasChartBiasFunction &biasFunction)
Sets bias function for sampling.
Definition: AtlasStateSpace.h:286
double rho_s_
Sampling radius within a chart. Inferred from rho and exploration parameters.
Definition: AtlasStateSpace.h:446
StateSamplerPtr allocDefaultStateSampler() const override
Allocate the default state sampler for this space.
Definition: AtlasStateSpace.h:202
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
double backoff_
Step size reduction factor during manifold traversal.
Definition: AtlasStateSpace.h:449
void setExploration(double exploration)
Set the exploration parameter, which tunes the balance of refinement (sampling within known regions) ...
Definition: AtlasStateSpace.h:266
AtlasChartBiasFunction biasFunction_
Function to bias chart sampling.
Definition: AtlasStateSpace.h:457
bool isSeparated() const
Returns whether the atlas is separating charts or not.
Definition: AtlasStateSpace.h:340
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:116
void setEpsilon(double epsilon)
Set epsilon, the maximum permissible distance between a point in the validity region of a chart and i...
Definition: AtlasStateSpace.h:231
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Definition: AtlasStateSpace.h:213
void printPLY(std::ostream &out) const
Write a mesh representation of the atlas to a stream.
Definition: AtlasStateSpace.cpp:508
StateType(const ConstrainedStateSpace *space)
Construct state of size n.
Definition: AtlasStateSpace.h:171
unsigned int maxChartsPerExtension_
Maximum number of charts that can be created in one manifold traversal.
Definition: AtlasStateSpace.h:452
A shared pointer wrapper for ompl::base::StateSpace.
void setRho(double rho)
Set rho, the maximum radius for which a chart is valid. Default 0.1.
Definition: AtlasStateSpace.h:241
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 ear...
Definition: AtlasStateSpace.cpp:370
A shared pointer wrapper for ompl::base::StateSampler.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Definition: WrapperStateSpace.h:333
void setSeparated(bool separate)
Sets whether the atlas should separate charts or not.
Definition: AtlasStateSpace.h:292
double estimateFrontierPercent() const
Estimate what percentage of atlas charts do not have fully formed polytope boundaries,...
Definition: AtlasStateSpace.cpp:499
double epsilon_
Maximum distance between a chart and the manifold inside its validity region.
Definition: AtlasStateSpace.h:434
double cos_alpha_
Cosine of the maximum angle between a chart and the manifold inside its validity region.
Definition: AtlasStateSpace.h:440
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 man...
Definition: AtlasStateSpace.cpp:339
void setMaxChartsPerExtension(unsigned int charts)
Sometimes manifold traversal creates many charts. This parameter limits the number of charts that can...
Definition: AtlasStateSpace.h:280
AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate=true)
Construct an atlas with the specified dimensions.
Definition: AtlasStateSpace.cpp:200
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 belo...
Definition: AtlasStateSpace.cpp:318
void sampleUniform(State *state) override
Sample a state uniformly from the charted regions of the manifold. Return sample in state.
Definition: AtlasStateSpace.cpp:51
std::size_t getChartCount() const
Return the number of charts currently in the atlas.
Definition: AtlasStateSpace.h:346
void setAlpha(double alpha)
Set alpha, the maximum permissible angle between the chart and the manifold inside the validity regio...
Definition: AtlasStateSpace.h:253