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 #include <eigen3/Eigen/Core>
50 
51 namespace ompl
52 {
53  namespace magic
54  {
55  static const unsigned int ATLAS_STATE_SPACE_SAMPLES = 50;
56  static const double ATLAS_STATE_SPACE_EPSILON = 0.05;
57  static const double ATLAS_STATE_SPACE_RHO_MULTIPLIER = 5;
58  static const double ATLAS_STATE_SPACE_ALPHA = boost::math::constants::pi<double>() / 8.0;
59  static const double ATLAS_STATE_SPACE_EXPLORATION = 0.75;
60  static const unsigned int ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION = 200;
61  static const double ATLAS_STATE_SPACE_BACKOFF = 0.75;
62  }
63 
64  namespace base
65  {
67 
68  OMPL_CLASS_FORWARD(AtlasChart);
70 
72 
73  OMPL_CLASS_FORWARD(AtlasStateSpace);
75 
78  {
79  public:
80  AtlasStateSampler(const AtlasStateSpace *space);
81 
84  void sampleUniform(State *state) override;
85 
89  void sampleUniformNear(State *state, const State *near, double distance) override;
90 
93  void sampleGaussian(State *state, const State *mean, double stdDev) override;
94 
95  private:
97  const AtlasStateSpace *atlas_;
98 
100  mutable RNG rng_;
101  };
102 
129  {
130  public:
131  typedef std::function<double(AtlasChart *)> AtlasChartBiasFunction;
132  using NNElement = std::pair<const StateType *, std::size_t>;
133 
137  {
138  public:
141  {
142  }
143 
146  {
147  return chart_;
148  }
149 
151  void setChart(AtlasChart *c) const
152  {
153  chart_ = c;
154  }
155 
156  private:
158  mutable AtlasChart *chart_{nullptr};
159  };
160 
162  AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate = true);
163 
165  ~AtlasStateSpace() override;
166 
168  void clear() override;
169 
172  {
173  return std::make_shared<AtlasStateSampler>(this);
174  }
175 
178  {
179  return std::make_shared<AtlasStateSampler>(this);
180  }
181 
182  void copyState(State *destination, const State *source) const override
183  {
184  ConstrainedStateSpace::copyState(destination, source);
185  destination->as<StateType>()->setChart(source->as<StateType>()->getChart());
186  }
187 
189  State *allocState() const override
190  {
191  return new StateType(this);
192  }
193 
200  void setEpsilon(double epsilon)
201  {
202  if (epsilon <= 0)
203  throw ompl::Exception("ompl::base::AtlasStateSpace::setEpsilon(): "
204  "epsilon must be positive.");
205  epsilon_ = epsilon;
206  }
207 
210  void setRho(double rho)
211  {
212  if (rho <= 0)
213  throw ompl::Exception("ompl::base::AtlasStateSpace::setRho(): "
214  "rho must be positive.");
215  rho_ = rho;
216  rho_s_ = rho_ / std::pow(1 - exploration_, 1.0 / k_);
217  }
218 
222  void setAlpha(double alpha)
223  {
224  if (alpha <= 0 || alpha >= boost::math::constants::pi<double>() / 2.)
225  throw ompl::Exception("ompl::base::AtlasStateSpace::setAlpha(): "
226  "alpha must be in (0, pi/2).");
227  cos_alpha_ = std::cos(alpha);
228  }
229 
235  void setExploration(double exploration)
236  {
237  if (exploration >= 1)
238  throw ompl::Exception("ompl::base::AtlasStateSpace::setExploration(): "
239  "exploration must be in [0, 1).");
240  exploration_ = exploration;
241 
242  // Update sampling radius
243  setRho(rho_);
244  }
245 
249  void setMaxChartsPerExtension(unsigned int charts)
250  {
251  maxChartsPerExtension_ = charts;
252  }
253 
255  void setBiasFunction(const AtlasChartBiasFunction &biasFunction)
256  {
257  biasFunction_ = biasFunction;
258  }
259 
261  void setSeparated(bool separate)
262  {
263  separate_ = separate;
264  }
265 
267  void setBackoff(double backoff)
268  {
269  backoff_ = backoff;
270  }
271 
273  double getEpsilon() const
274  {
275  return epsilon_;
276  }
277 
279  double getRho() const
280  {
281  return rho_;
282  }
283 
285  double getAlpha() const
286  {
287  return std::acos(cos_alpha_);
288  }
289 
291  double getExploration() const
292  {
293  return exploration_;
294  }
295 
297  double getRho_s() const
298  {
299  return rho_s_;
300  }
301 
303  unsigned int getMaxChartsPerExtension() const
304  {
305  return maxChartsPerExtension_;
306  }
307 
309  bool isSeparated() const
310  {
311  return separate_;
312  }
313 
315  std::size_t getChartCount() const
316  {
317  return charts_.size();
318  }
319 
321  double getBackoff() const
322  {
323  return backoff_;
324  }
325 
333  AtlasChart *newChart(const StateType *state) const;
334 
336  AtlasChart *sampleChart() const;
337 
340  AtlasChart *owningChart(const StateType *state) const;
341 
347  AtlasChart *getChart(const StateType *state, bool force = false, bool *created = nullptr) const;
348 
357  AtlasChart *anchorChart(const State *state) const;
358 
367  bool discreteGeodesic(const State *from, const State *to, bool interpolate = false,
368  std::vector<State *> *geodesic = nullptr) const override;
369 
378  double estimateFrontierPercent() const;
379 
381  void printPLY(std::ostream &out) const;
382 
385  protected:
387  mutable std::vector<StateType *> anchors_;
388 
390  mutable std::vector<AtlasChart *> charts_;
391 
394 
398 
403  double epsilon_{ompl::magic::ATLAS_STATE_SPACE_EPSILON};
404 
406  double rho_;
407 
409  double cos_alpha_;
410 
412  double exploration_;
413 
415  mutable double rho_s_;
416 
418  double backoff_{ompl::magic::ATLAS_STATE_SPACE_BACKOFF};
419 
421  unsigned int maxChartsPerExtension_{ompl::magic::ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION};
422 
426  AtlasChartBiasFunction biasFunction_;
427 
429  bool separate_;
430 
432  mutable RNG rng_;
433  };
434  }
435 }
436 
437 #endif
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
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...
const unsigned int k_
Manifold dimension.
std::size_t getChartCount() const
Return the number of charts currently in the atlas.
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 getAlpha() const
Get alpha.
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...
State * allocState() const override
Allocate a new state in this space.
double rho_s_
Sampling radius within a chart. Inferred from rho and exploration parameters.
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
void setMaxChartsPerExtension(unsigned int charts)
Sometimes manifold traversal creates many charts. This parameter limits the number of charts that can...
bool isSeparated() const
Returns whether the atlas is separating charts or not.
void printPLY(std::ostream &out) const
Write a mesh representation of the atlas to a stream.
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to...
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:52
A shared pointer wrapper for ompl::base::StateSampler.
double estimateFrontierPercent() const
Estimate what percentage of atlas charts do not have fully formed polytope boundaries, and are therefore on the frontier.
double getExploration() const
Get the exploration parameter.
AtlasChart * anchorChart(const State *state) const
Wrapper for newChart(). Charts created this way will persist through calls to clear().
AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate=true)
Construct an atlas with the specified dimensions.
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...
NearestNeighborsGNAT< NNElement > chartNN_
Set of chart centers and indices, accessible by nearest-neighbor queries to the chart centers...
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
StateSamplerPtr allocStateSampler() const override
Allocate the previously set state sampler for this space.
void setSeparated(bool separate)
Sets whether the atlas should separate charts or not.
double cos_alpha_
Cosine of the maximum angle between a chart and the manifold inside its validity region.
~AtlasStateSpace() override
Destructor.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
unsigned int getMaxChartsPerExtension() const
Get the maximum number of charts to create in one pass.
PDF< AtlasChart * > chartPDF_
PDF of charts according to a bias function.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void setChart(AtlasChart *c) const
Set the chart c for the state.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
StateSamplerPtr allocDefaultStateSampler() const override
Allocate the default state sampler for this space.
std::vector< StateType * > anchors_
Set of states on which there are anchored charts.
double getRho_s() const
Get the sampling radius.
double getEpsilon() const
Get epsilon.
std::vector< AtlasChart * > charts_
Set of charts.
double getRho() const
Get rho.
void clear() override
Reset the space (except for anchor charts).
void setRho(double rho)
Set rho, the maximum radius for which a chart is valid. Default 0.1.
StateSampler for use on an atlas.
Definition of an abstract state.
Definition: State.h:49
void setBackoff(double backoff)
Sets the backoff factor in manifold traversal factor.
void setEpsilon(double epsilon)
Set epsilon, the maximum permissible distance between a point in the validity region of a chart and i...
The exception type for ompl.
Definition: Exception.h:46
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.
double rho_
Maximum radius of chart validity region.
bool separate_
Enable or disable halfspace separation of the charts.
AtlasStateSampler(const AtlasStateSpace *space)
AtlasStateSampler.
void setAlpha(double alpha)
Set alpha, the maximum permissible angle between the chart and the manifold inside the validity regio...
AtlasChartBiasFunction biasFunction_
Function to bias chart sampling.
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
StateType(const ConstrainedStateSpace *space)
Construct state of size n.
void setExploration(double exploration)
Set the exploration parameter, which tunes the balance of refinement (sampling within known regions) ...
unsigned int maxChartsPerExtension_
Maximum number of charts that can be created in one manifold traversal.
RNG rng_
Random number generator.
Abstract definition of a state space sampler.
Definition: StateSampler.h:64
void sampleUniform(State *state) override
Sample a state uniformly from the charted regions of the manifold. Return sample in state...
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...
double epsilon_
Maximum distance between a chart and the manifold inside its validity region.
AtlasChart * sampleChart() const
Pick a chart at random.
void setBiasFunction(const AtlasChartBiasFunction &biasFunction)
Sets bias function for sampling.
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.
double getBackoff() const
Returns the current backoff factor used in manifold traversal.
AtlasChart * getChart() const
Get the chart this state is on.
double backoff_
Step size reduction factor during manifold traversal.