AtlasStateSpace.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2014, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Zachary Kingston, Caleb Voss */
36
37#ifndef OMPL_BASE_SPACES_ATLAS_STATE_SPACE_
38#define OMPL_BASE_SPACES_ATLAS_STATE_SPACE_
39
40#include "ompl/base/StateSampler.h"
41#include "ompl/base/ValidStateSampler.h"
42#include "ompl/base/Constraint.h"
43#include "ompl/datastructures/NearestNeighborsGNAT.h"
44#include "ompl/datastructures/PDF.h"
45
46#include "ompl/base/spaces/constraint/ConstrainedStateSpace.h"
47
48#include <boost/math/constants/constants.hpp>
49
50namespace ompl
51{
52 namespace magic
53 {
54 static const unsigned int ATLAS_STATE_SPACE_SAMPLES = 50;
55 static const double ATLAS_STATE_SPACE_EPSILON = 0.05;
56 static const double ATLAS_STATE_SPACE_RHO_MULTIPLIER = 5;
57 static const double ATLAS_STATE_SPACE_ALPHA = boost::math::constants::pi<double>() / 8.0;
58 static const double ATLAS_STATE_SPACE_EXPLORATION = 0.75;
59 static const unsigned int ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION = 200;
60 static const double ATLAS_STATE_SPACE_BACKOFF = 0.75;
61 }
62
63 namespace base
64 {
66
67 OMPL_CLASS_FORWARD(AtlasChart);
69
71
72 OMPL_CLASS_FORWARD(AtlasStateSpace);
74
77 {
78 public:
80
83 void sampleUniform(State *state) override;
84
88 void sampleUniformNear(State *state, const State *near, double distance) override;
89
92 void sampleGaussian(State *state, const State *mean, double stdDev) override;
93
94 private:
96 const AtlasStateSpace *atlas_;
97
99 mutable RNG rng_;
100 };
101
128 {
129 public:
130 using AtlasChartBiasFunction = std::function<double(AtlasChart *)>;
131 using NNElement = std::pair<const StateType *, std::size_t>;
132
136 {
137 public:
140 {
141 }
142
145 {
146 return chart_;
147 }
148
150 void setChart(AtlasChart *c) const
151 {
152 chart_ = c;
153 }
154
155 private:
157 mutable AtlasChart *chart_{nullptr};
158 };
159
161 AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate = true);
162
164 ~AtlasStateSpace() override;
165
167 void clear() override;
168
171 {
172 return std::make_shared<AtlasStateSampler>(this);
173 }
174
177 {
178 return std::make_shared<AtlasStateSampler>(this);
179 }
180
181 void copyState(State *destination, const State *source) const override
182 {
183 ConstrainedStateSpace::copyState(destination, source);
184 destination->as<StateType>()->setChart(source->as<StateType>()->getChart());
185 }
186
188 State *allocState() const override
189 {
190 return new StateType(this);
191 }
192
199 void setEpsilon(double epsilon)
200 {
201 if (epsilon <= 0)
202 throw ompl::Exception("ompl::base::AtlasStateSpace::setEpsilon(): "
203 "epsilon must be positive.");
204 epsilon_ = epsilon;
205 }
206
209 void setRho(double rho)
210 {
211 if (rho <= 0)
212 throw ompl::Exception("ompl::base::AtlasStateSpace::setRho(): "
213 "rho must be positive.");
214 rho_ = rho;
215 rho_s_ = rho_ / std::pow(1 - exploration_, 1.0 / k_);
216 }
217
221 void setAlpha(double alpha)
222 {
223 if (alpha <= 0 || alpha >= boost::math::constants::pi<double>() / 2.)
224 throw ompl::Exception("ompl::base::AtlasStateSpace::setAlpha(): "
225 "alpha must be in (0, pi/2).");
226 cos_alpha_ = std::cos(alpha);
227 }
228
234 void setExploration(double exploration)
235 {
236 if (exploration >= 1)
237 throw ompl::Exception("ompl::base::AtlasStateSpace::setExploration(): "
238 "exploration must be in [0, 1).");
239 exploration_ = exploration;
240
241 // Update sampling radius
242 setRho(rho_);
243 }
244
248 void setMaxChartsPerExtension(unsigned int charts)
249 {
250 maxChartsPerExtension_ = charts;
251 }
252
254 void setBiasFunction(const AtlasChartBiasFunction &biasFunction)
255 {
256 biasFunction_ = biasFunction;
257 }
258
260 void setSeparated(bool separate)
261 {
262 separate_ = separate;
263 }
264
266 void setBackoff(double backoff)
267 {
268 backoff_ = backoff;
269 }
270
272 double getEpsilon() const
273 {
274 return epsilon_;
275 }
276
278 double getRho() const
279 {
280 return rho_;
281 }
282
284 double getAlpha() const
285 {
286 return std::acos(cos_alpha_);
287 }
288
290 double getExploration() const
291 {
292 return exploration_;
293 }
294
296 double getRho_s() const
297 {
298 return rho_s_;
299 }
300
302 unsigned int getMaxChartsPerExtension() const
303 {
305 }
306
308 bool isSeparated() const
309 {
310 return separate_;
311 }
312
314 std::size_t getChartCount() const
315 {
316 return charts_.size();
317 }
318
320 double getBackoff() const
321 {
322 return backoff_;
323 }
324
332 AtlasChart *newChart(const StateType *state) const;
333
335 AtlasChart *sampleChart() const;
336
339 AtlasChart *owningChart(const StateType *state) const;
340
346 AtlasChart *getChart(const StateType *state, bool force = false, bool *created = nullptr) const;
347
356 AtlasChart *anchorChart(const State *state) const;
357
366 bool discreteGeodesic(const State *from, const State *to, bool interpolate = false,
367 std::vector<State *> *geodesic = nullptr) const override;
368
377 double estimateFrontierPercent() const;
378
380 void printPLY(std::ostream &out) const;
381
384 protected:
386 mutable std::vector<StateType *> anchors_;
387
389 mutable std::vector<AtlasChart *> charts_;
390
393
397
402 double epsilon_{ompl::magic::ATLAS_STATE_SPACE_EPSILON};
403
405 double rho_;
406
409
412
414 mutable double rho_s_;
415
417 double backoff_{ompl::magic::ATLAS_STATE_SPACE_BACKOFF};
418
420 unsigned int maxChartsPerExtension_{ompl::magic::ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION};
421
425 AtlasChartBiasFunction biasFunction_;
426
429
431 mutable RNG rng_;
432 };
433 }
434}
435
436#endif
The exception type for ompl.
Definition: Exception.h:47
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:49
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:53
StateSampler for use on an atlas.
void sampleUniform(State *state) override
Sample a state uniformly from the charted regions of the manifold. Return sample in state.
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...
AtlasStateSampler(const AtlasStateSpace *space)
AtlasStateSampler.
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.
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to.
AtlasChart * getChart() const
Get the chart this state is on.
StateType(const ConstrainedStateSpace *space)
Construct state of size n.
void setChart(AtlasChart *c) const
Set the chart c for the state.
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
bool isSeparated() const
Returns whether the atlas is separating charts or not.
void clear() override
Reset the space (except for anchor charts).
std::vector< StateType * > anchors_
Set of states on which there are anchored charts.
~AtlasStateSpace() override
Destructor.
unsigned int maxChartsPerExtension_
Maximum number of charts that can be created in one manifold traversal.
StateSamplerPtr allocStateSampler() const override
Allocate the previously set state sampler for this space.
AtlasChart * sampleChart() const
Pick a chart at random.
double estimateFrontierPercent() const
Estimate what percentage of atlas charts do not have fully formed polytope boundaries,...
double cos_alpha_
Cosine of the maximum angle between a chart and the manifold inside its validity region.
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...
double getRho() const
Get rho.
PDF< AtlasChart * > chartPDF_
PDF of charts according to a bias function.
double getRho_s() const
Get the sampling radius.
double getExploration() const
Get the exploration parameter.
std::vector< AtlasChart * > charts_
Set of charts.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
void setRho(double rho)
Set rho, the maximum radius for which a chart is valid. Default 0.1.
double getAlpha() const
Get alpha.
double getBackoff() const
Returns the current backoff factor used in manifold traversal.
void printPLY(std::ostream &out) const
Write a mesh representation of the atlas to a stream.
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...
AtlasChart * anchorChart(const State *state) const
Wrapper for newChart(). Charts created this way will persist through calls to clear().
double epsilon_
Maximum distance between a chart and the manifold inside its validity region.
double rho_s_
Sampling radius within a chart. Inferred from rho and exploration parameters.
void setExploration(double exploration)
Set the exploration parameter, which tunes the balance of refinement (sampling within known regions) ...
std::size_t getChartCount() const
Return the number of charts currently in the atlas.
double backoff_
Step size reduction factor during manifold traversal.
NearestNeighborsGNAT< NNElement > chartNN_
Set of chart centers and indices, accessible by nearest-neighbor queries to the chart centers.
void setMaxChartsPerExtension(unsigned int charts)
Sometimes manifold traversal creates many charts. This parameter limits the number of charts that can...
void setBiasFunction(const AtlasChartBiasFunction &biasFunction)
Sets bias function for sampling.
RNG rng_
Random number generator.
double exploration_
Balance between explorationa and refinement.
AtlasChartBiasFunction biasFunction_
Function to bias chart sampling.
void setSeparated(bool separate)
Sets whether the atlas should separate charts or not.
AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate=true)
Construct an atlas with the specified dimensions.
void setAlpha(double alpha)
Set alpha, the maximum permissible angle between the chart and the manifold inside the validity regio...
void setEpsilon(double epsilon)
Set epsilon, the maximum permissible distance between a point in the validity region of a chart and i...
AtlasChart * newChart(const StateType *state) const
Create a new chart for the atlas, centered at xorigin, which should be on the manifold....
StateSamplerPtr allocDefaultStateSampler() const override
Allocate the default state sampler for this space.
double getEpsilon() const
Get epsilon.
unsigned int getMaxChartsPerExtension() const
Get the maximum number of charts to create in one pass.
bool separate_
Enable or disable halfspace separation of the charts.
double rho_
Maximum radius of chart validity region.
State * allocState() const override
Allocate a new state in this space.
void setBackoff(double backoff)
Sets the backoff factor in manifold traversal factor.
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...
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
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...
const unsigned int k_
Manifold dimension.
A shared pointer wrapper for ompl::base::Constraint.
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
Definition: StateSampler.h:65
A shared pointer wrapper for ompl::base::StateSpace.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Main namespace. Contains everything in this library.