AtlasStateSpace.cpp
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 #include "ompl/base/spaces/constraint/AtlasStateSpace.h"
38 #include "ompl/base/spaces/constraint/AtlasChart.h"
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/util/Exception.h"
42 
44 
46 
48 {
49 }
50 
52 {
53  auto astate = state->as<AtlasStateSpace::StateType>();
54 
55  const std::size_t k = atlas_->getManifoldDimension();
56  Eigen::VectorXd ru(k);
57 
58  AtlasChart *c;
59 
60  // Sampling a point on the manifold.
61  unsigned int tries = ompl::magic::ATLAS_STATE_SPACE_SAMPLES;
62  do
63  {
64  // Rejection sampling to find a point inside a chart's polytope.
65  do
66  {
67  // Pick a chart.
68  c = atlas_->sampleChart();
69 
70  // Sample a point within rho_s of the center. This is done by
71  // sampling uniformly on the surface and multiplying by a dist
72  // whose distribution is biased according to spherical volume.
73  for (std::size_t i = 0; i < k; ++i)
74  ru[i] = rng_.gaussian01();
75 
76  ru *= atlas_->getRho_s() * std::pow(rng_.uniform01(), 1.0 / k) / ru.norm();
77  } while (tries-- > 0 && !c->inPolytope(ru));
78 
79  // Project. Will need to try again if this fails.
80  } while (tries > 0 && !c->psi(ru, *astate));
81 
82  if (tries == 0)
83  {
84  // Consider decreasing rho and/or the exploration paramter if this
85  // becomes a problem.
86  OMPL_WARN("ompl::base::AtlasStateSpace::sampleUniform(): "
87  "Took too long; returning center of a random chart.");
88  atlas_->copyState(astate, c->getOrigin());
89  }
90 
91  space_->enforceBounds(state);
92 
93  // Extend polytope of neighboring chart wherever point is near the border.
94  c->psiInverse(*astate, ru);
95  c->borderCheck(ru);
96  astate->setChart(atlas_->owningChart(astate));
97 }
98 
99 void ompl::base::AtlasStateSampler::sampleUniformNear(State *state, const State *near, const double dist)
100 {
101  // Find the chart that the starting point is on.
102  auto astate = state->as<AtlasStateSpace::StateType>();
103  auto anear = near->as<AtlasStateSpace::StateType>();
104 
105  const std::size_t k = atlas_->getManifoldDimension();
106 
107  Eigen::VectorXd ru(k), uoffset(k);
108 
109  AtlasChart *c = atlas_->getChart(anear, true);
110  if (c == nullptr)
111  {
112  OMPL_ERROR("ompl::base::AtlasStateSpace::sampleUniformNear(): "
113  "Sampling failed because chart creation failed! Falling back to uniform sample.");
114  sampleUniform(state);
115  return;
116  }
117 
118  // Sample a point from the starting chart.
119  c->psiInverse(*anear, ru);
120  unsigned int tries = ompl::magic::ATLAS_STATE_SPACE_SAMPLES;
121  do
122  {
123  // Sample within dist
124  for (std::size_t i = 0; i < k; ++i)
125  uoffset[i] = ru[i] + rng_.gaussian01();
126 
127  uoffset *= dist * std::pow(rng_.uniform01(), 1.0 / k) / uoffset.norm();
128  } while (--tries > 0 && !c->psi(uoffset, *astate)); // Try again if we can't project.
129 
130  if (tries == 0)
131  {
132  // Consider decreasing the dist argument if this becomes a
133  // problem. Check planner code to see how it gets chosen.
134  OMPL_WARN("ompl::base:::AtlasStateSpace::sampleUniformNear(): "
135  "Took too long; returning initial point.");
136  atlas_->copyState(state, near);
137  }
138 
139  space_->enforceBounds(state);
140 
141  c->psiInverse(*astate, ru);
142  if (!c->inPolytope(ru))
143  c = atlas_->getChart(astate, true);
144  else
145  c->borderCheck(ru);
146 
147  astate->setChart(c);
148 }
149 
150 void ompl::base::AtlasStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
151 {
152  auto astate = state->as<AtlasStateSpace::StateType>();
153  auto amean = mean->as<AtlasStateSpace::StateType>();
154 
155  const std::size_t k = atlas_->getManifoldDimension();
156  Eigen::VectorXd ru(k), rand(k);
157 
158  AtlasChart *c = atlas_->getChart(amean, true);
159  if (c == nullptr)
160  {
161  OMPL_ERROR("ompl::base::AtlasStateSpace::sampleGaussian(): "
162  "Sampling failed because chart creation failed! Falling back to uniform sample.");
163  sampleUniform(state);
164  return;
165  }
166 
167  c->psiInverse(*amean, ru);
168 
169  // Sample a point in a normal distribution on the starting chart.
170  unsigned int tries = ompl::magic::ATLAS_STATE_SPACE_SAMPLES;
171 
172  do
173  {
174  for (std::size_t i = 0; i < k; i++)
175  rand[i] = ru[i] + rng_.gaussian(0, stdDev);
176  } while (--tries > 0 && !c->psi(rand, *astate)); // Try again if we can't project.
177 
178  if (tries == 0)
179  {
180  OMPL_WARN("ompl::base::AtlasStateSpace::sampleUniforGaussian(): "
181  "Took too long; returning initial point.");
182  atlas_->copyState(state, mean);
183  }
184 
185  space_->enforceBounds(state);
186 
187  c->psiInverse(*astate, ru);
188  if (!c->inPolytope(ru))
189  c = atlas_->getChart(astate, true);
190  else
191  c->borderCheck(ru);
192 
193  astate->setChart(c);
194 }
195 
197 
199 
201  bool separate)
202  : ConstrainedStateSpace(ambientSpace, constraint)
203  , biasFunction_([](AtlasChart *c) -> double { return 1; })
204  , separate_(separate)
205 {
206  setRho(delta_ * ompl::magic::ATLAS_STATE_SPACE_RHO_MULTIPLIER);
207  setAlpha(ompl::magic::ATLAS_STATE_SPACE_ALPHA);
208  setExploration(ompl::magic::ATLAS_STATE_SPACE_EXPLORATION);
209 
210  setName("Atlas" + space_->getName());
211 
212  chartNN_.setDistanceFunction(
213  [&](const NNElement &e1, const NNElement &e2) -> double { return distance(e1.first, e2.first); });
214 }
215 
217 {
218  // Delete anchors first so clear does no reinitialization.
219  for (auto anchor : anchors_)
220  freeState(anchor);
221 
222  anchors_.clear();
223  clear();
224 }
225 
227 {
228  // Delete the non-anchor charts
229  for (auto chart : charts_)
230  delete chart;
231  charts_.clear();
232 
233  std::vector<NNElement> nnList;
234  chartNN_.list(nnList);
235  for (auto &chart : nnList)
236  {
237  const State *state = chart.first;
238  freeState(const_cast<State *>(state));
239  }
240 
241  chartNN_.clear();
242  chartPDF_.clear();
243 
244  // Reinstate the anchor charts
245  for (auto anchor : anchors_)
246  newChart(anchor);
247 
249 }
250 
252 {
253  auto anchor = cloneState(state)->as<StateType>();
254  anchors_.push_back(anchor);
255 
256  // This could fail with an exception. We cannot recover if that happens.
257  AtlasChart *chart = newChart(anchor);
258  if (chart == nullptr)
259  throw ompl::Exception("ompl::base::AtlasStateSpace::anchorChart(): "
260  "Initial chart creation failed. Cannot proceed.");
261 
262  return chart;
263 }
264 
266 {
267  AtlasChart *chart;
268  StateType *cstate = nullptr;
269 
270  try
271  {
272  cstate = cloneState(state)->as<StateType>();
273  chart = new AtlasChart(this, cstate);
274  }
275  catch (ompl::Exception &e)
276  {
277  OMPL_ERROR("ompl::base::AtlasStateSpace::newChart(): "
278  "Failed because manifold looks degenerate here.");
279 
280  if (cstate != nullptr)
281  freeState(cstate);
282 
283  return nullptr;
284  }
285 
286  // Ensure all charts respect boundaries of the new one, and vice versa, but
287  // only look at nearby ones (within 2*rho).
288  if (separate_)
289  {
290  std::vector<NNElement> nearbyCharts;
291  chartNN_.nearestR(std::make_pair(cstate, 0), 2 * rho_s_, nearbyCharts);
292 
293  for (auto &&near : nearbyCharts)
294  {
295  AtlasChart *other = charts_[near.second];
296  AtlasChart::generateHalfspace(other, chart);
297 
298  chartPDF_.update(chartPDF_.getElements()[near.second], biasFunction_(other));
299  }
300  }
301 
302  chartNN_.add(std::make_pair(cstate, charts_.size()));
303  charts_.push_back(chart);
304  chartPDF_.add(chart, biasFunction_(chart));
305 
306  return chart;
307 }
308 
310 {
311  if (charts_.empty())
312  throw ompl::Exception("ompl::base::AtlasStateSpace::sampleChart(): "
313  "Atlas sampled before any charts were made. Use AtlasStateSpace::anchorChart() first.");
314 
315  return chartPDF_.sample(rng_.uniform01());
316 }
317 
318 ompl::base::AtlasChart *ompl::base::AtlasStateSpace::getChart(const StateType *state, bool force, bool *created) const
319 {
320  AtlasChart *c = state->getChart();
321  if (c == nullptr || force)
322  {
323  c = owningChart(state);
324 
325  if (c == nullptr)
326  {
327  c = newChart(state);
328  if (created != nullptr)
329  *created = true;
330  }
331 
332  if (c != nullptr)
333  state->setChart(c);
334  }
335 
336  return c;
337 }
338 
340 {
341  Eigen::VectorXd u_t(k_);
342  auto temp = allocState()->as<StateType>();
343 
344  std::vector<NNElement> nearby;
345  chartNN_.nearestR(std::make_pair(state, 0), rho_, nearby);
346 
347  double best = epsilon_;
348  AtlasChart *chart = nullptr;
349  for (auto near = nearby.begin(); near != nearby.end(); ++near)
350  {
351  // The point must lie in the chart's validity region and polytope
352  auto owner = charts_[near->second];
353  owner->psiInverse(*state, u_t);
354  owner->phi(u_t, *temp);
355 
356  double far;
357  if (owner->inPolytope(u_t) // in polytope
358  && (far = distance(state, temp)) < epsilon_ // within epsilon
359  && far < best)
360  {
361  best = far;
362  chart = owner;
363  }
364  }
365 
366  freeState(temp);
367  return chart;
368 }
369 
370 bool ompl::base::AtlasStateSpace::discreteGeodesic(const State *from, const State *to, bool interpolate,
371  std::vector<ompl::base::State *> *geodesic) const
372 {
373  auto &&svc = si_->getStateValidityChecker();
374 
375  // We can't traverse the manifold if we don't start on it.
376  if (!constraint_->isSatisfied(from) || !(interpolate || svc->isValid(from)))
377  return false;
378 
379  auto afrom = from->as<StateType>();
380  auto ato = to->as<StateType>();
381 
382  // Try to get starting chart from `from` state.
383  AtlasChart *c = getChart(afrom);
384  if (c == nullptr)
385  return false;
386 
387  // Save a copy of the from state.
388  if (geodesic != nullptr)
389  {
390  geodesic->clear();
391  geodesic->push_back(cloneState(from));
392  }
393 
394  // No need to traverse the manifold if we are already there
395  const double tolerance = delta_;
396  const double distTo = distance(from, to);
397  if (distTo <= tolerance)
398  return true;
399 
400  // Traversal stops if the ball of radius distMax centered at x_from is left
401  const double distMax = lambda_ * distTo;
402 
403  // Create a scratch state to use for movement.
404  auto scratch = cloneState(from)->as<StateType>();
405  auto temp = allocState()->as<StateType>();
406 
407  // Project from and to points onto the chart
408  Eigen::VectorXd u_j(k_), u_b(k_);
409  c->psiInverse(*scratch, u_j);
410  c->psiInverse(*ato, u_b);
411 
412  bool done = false;
413  std::size_t chartsCreated = 0;
414  double dist = 0;
415 
416  double factor = 1;
417 
418  do
419  {
420  if (factor < delta_)
421  break;
422 
423  // Take a step towards the final state
424  u_j += factor * delta_ * (u_b - u_j).normalized();
425 
426  // will also bork on non-finite numbers
427  const bool onManifold = c->psi(u_j, *temp);
428  if (!onManifold)
429  {
430  done = false;
431  break;
432  }
433 
434  const double step = distance(scratch, temp);
435 
436  if (step < std::numeric_limits<double>::epsilon())
437  break;
438 
439  const bool exceedStepSize = step >= lambda_ * delta_;
440  if (exceedStepSize)
441  {
442  factor *= backoff_;
443  continue;
444  }
445 
446  dist += step;
447 
448  // Update state
449  copyState(scratch, temp);
450  scratch->setChart(c);
451 
452  if (!(interpolate || svc->isValid(scratch)) // not valid
453  || distance(from, scratch) > distMax // exceed max dist
454  || dist > distMax // exceed wandering
455  || chartsCreated > maxChartsPerExtension_) // exceed chart limit
456  {
457  done = false;
458  break;
459  }
460 
461  // Check if we left the validity region or polytope of the chart.
462  c->phi(u_j, *temp);
463 
464  // Find or make a new chart if new state is off of current chart
465  if (distance(scratch, temp) > epsilon_ // exceeds epsilon
466  || delta_ / step < cos_alpha_ // exceeds angle
467  || !c->inPolytope(u_j)) // outside polytope
468  {
469  bool created = false;
470  if ((c = getChart(scratch, true, &created)) == nullptr)
471  {
472  OMPL_ERROR("Treating singularity as an obstacle.");
473  done = false;
474  break;
475  }
476  chartsCreated += created;
477 
478  // Re-project onto the next chart.
479  c->psiInverse(*scratch, u_j);
480  c->psiInverse(*ato, u_b);
481  }
482 
483  done = distance(scratch, to) <= delta_;
484  factor = 1;
485 
486  // Keep the state in a list, if requested.
487  if (geodesic != nullptr)
488  geodesic->push_back(cloneState(scratch));
489 
490  } while (!done);
491 
492  const bool ret = done && distance(to, scratch) <= delta_;
493  freeState(scratch);
494  freeState(temp);
495 
496  return ret;
497 }
498 
500 {
501  double frontier = 0;
502  for (const AtlasChart *c : charts_)
503  frontier += c->estimateIsFrontier() ? 1 : 0;
504 
505  return (100 * frontier) / charts_.size();
506 }
507 
508 void ompl::base::AtlasStateSpace::printPLY(std::ostream &out) const
509 {
510  std::stringstream v, f;
511  std::size_t vcount = 0;
512  std::size_t fcount = 0;
513  std::vector<Eigen::VectorXd> vertices;
514  for (AtlasChart *c : charts_)
515  {
516  vertices.clear();
517  c->toPolygon(vertices);
518 
519  std::stringstream poly;
520  std::size_t fvcount = 0;
521  for (Eigen::VectorXd &vert : vertices)
522  {
523  v << vert.transpose() << "\n";
524  poly << vcount++ << " ";
525  fvcount++;
526  }
527 
528  if (fvcount > 2)
529  {
530  f << fvcount << " " << poly.str() << "\n";
531  fcount += 1;
532  }
533  }
534  vertices.clear();
535 
536  out << "ply\n";
537  out << "format ascii 1.0\n";
538  out << "element vertex " << vcount << "\n";
539  out << "property float x\n";
540  out << "property float y\n";
541  out << "property float z\n";
542  out << "element face " << fcount << "\n";
543  out << "property list uint uint vertex_index\n";
544  out << "end_header\n";
545  out << v.str() << f.str();
546 }
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...
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...
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...
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
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...
A shared pointer wrapper for ompl::base::StateSpace.
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:52
double estimateFrontierPercent() const
Estimate what percentage of atlas charts do not have fully formed polytope boundaries, and are therefore on the frontier.
AtlasChart * anchorChart(const State *state) const
Wrapper for newChart(). Charts created this way will persist through calls to clear().
A shared pointer wrapper for ompl::base::Constraint.
AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate=true)
Construct an atlas with the specified dimensions.
void psiInverse(const Eigen::Ref< const Eigen::VectorXd > &x, Eigen::Ref< Eigen::VectorXd > out) const
Logarithmic mapping. Project ambient point x onto the chart and store the result in out...
Definition: AtlasChart.cpp:232
~AtlasStateSpace() override
Destructor.
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.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void clear() override
Reset the space (except for anchor charts).
static void generateHalfspace(AtlasChart *c1, AtlasChart *c2)
Create two complementary halfspaces dividing the space between charts c1 and c2, and add them to the ...
Definition: AtlasChart.cpp:369
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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.
AtlasStateSampler(const AtlasStateSpace *space)
AtlasStateSampler.
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
Abstract definition of a state space sampler.
Definition: StateSampler.h:64
virtual void clear()
Clear any allocated memory from the state space.
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...
AtlasChart * sampleChart() const
Pick a chart at random.
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...
AtlasChart * getChart() const
Get the chart this state is on.