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 
50 namespace 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:
79  AtlasStateSampler(const AtlasStateSpace *space);
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  {
304  return maxChartsPerExtension_;
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 
408  double cos_alpha_;
409 
411  double exploration_;
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 
428  bool separate_;
429 
431  mutable RNG rng_;
432  };
433  }
434 }
435 
436 #endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
AtlasChart * newChart(const StateType *state) const
Create a new chart for the atlas, centered at xorigin, which should be on the manifold....
double getBackoff() const
Returns the current backoff factor used in manifold traversal.
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to.
PDF< AtlasChart * > chartPDF_
PDF of charts according to a bias function.
StateSampler for use on an atlas.
State * allocState() const override
Allocate a new state in this space.
StateSamplerPtr allocStateSampler() const override
Allocate the previously set state sampler for this space.
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...
unsigned int getMaxChartsPerExtension() const
Get the maximum number of charts to create in one pass.
NearestNeighborsGNAT< NNElement > chartNN_
Set of chart centers and indices, accessible by nearest-neighbor queries to the chart centers.
std::vector< StateType * > anchors_
Set of states on which there are anchored charts.
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 of an abstract state.
Definition: State.h:50
bool separate_
Enable or disable halfspace separation of the charts.
double getExploration() const
Get the exploration parameter.
void setBackoff(double backoff)
Sets the backoff factor in manifold traversal factor.
void clear() override
Reset the space (except for anchor charts).
double getRho_s() const
Get the sampling radius.
AtlasChart * anchorChart(const State *state) const
Wrapper for newChart(). Charts created this way will persist through calls to clear().
RNG rng_
Random number generator.
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...
void setBiasFunction(const AtlasChartBiasFunction &biasFunction)
Sets bias function for sampling.
double rho_s_
Sampling radius within a chart. Inferred from rho and exploration parameters.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:49
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
StateSamplerPtr allocDefaultStateSampler() const override
Allocate the default state sampler for this space.
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
double backoff_
Step size reduction factor during manifold traversal.
AtlasChart * sampleChart() const
Pick a chart at random.
double getAlpha() const
Get alpha.
void setChart(AtlasChart *c) const
Set the chart c for the state.
double getEpsilon() const
Get epsilon.
double rho_
Maximum radius of chart validity region.
void setExploration(double exploration)
Set the exploration parameter, which tunes the balance of refinement (sampling within known regions) ...
const unsigned int k_
Manifold dimension.
double getRho() const
Get rho.
std::vector< AtlasChart * > charts_
Set of charts.
AtlasChartBiasFunction biasFunction_
Function to bias chart sampling.
bool isSeparated() const
Returns whether the atlas is separating charts or not.
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:53
void setEpsilon(double epsilon)
Set epsilon, the maximum permissible distance between a point in the validity region of a chart and i...
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
void printPLY(std::ostream &out) const
Write a mesh representation of the atlas to a stream.
StateType(const ConstrainedStateSpace *space)
Construct state of size n.
unsigned int maxChartsPerExtension_
Maximum number of charts that can be created in one manifold traversal.
Abstract definition of a state space sampler.
Definition: StateSampler.h:65
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
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.
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 * getChart() const
Get the chart this state is on.
A shared pointer wrapper for ompl::base::StateSampler.
AtlasStateSampler(const AtlasStateSpace *space)
AtlasStateSampler.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
~AtlasStateSpace() override
Destructor.
void setSeparated(bool separate)
Sets whether the atlas should separate charts or not.
The exception type for ompl.
Definition: Exception.h:47
double estimateFrontierPercent() const
Estimate what percentage of atlas charts do not have fully formed polytope boundaries,...
double epsilon_
Maximum distance between a chart and the manifold inside its validity region.
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...
void setMaxChartsPerExtension(unsigned int charts)
Sometimes manifold traversal creates many charts. This parameter limits the number of charts that can...
AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate=true)
Construct an atlas with the specified dimensions.
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...
double exploration_
Balance between explorationa and refinement.
void sampleUniform(State *state) override
Sample a state uniformly from the charted regions of the manifold. Return sample in state.
std::size_t getChartCount() const
Return the number of charts currently in the atlas.
void setAlpha(double alpha)
Set alpha, the maximum permissible angle between the chart and the manifold inside the validity regio...
Main namespace. Contains everything in this library.
Definition: AppBase.h:22