Loading...
Searching...
No Matches
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
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
99void 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
150void 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 *) -> 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
318ompl::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)
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
370bool 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
508void 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}
The exception type for ompl.
Definition Exception.h:47
Tangent space and bounding polytope approximating some patch of the manifold.
Definition AtlasChart.h:53
static void generateHalfspace(AtlasChart *c1, AtlasChart *c2)
Create two complementary halfspaces dividing the space between charts c1 and c2, and add them to the ...
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,...
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.
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...
void clear() override
Reset the space (except for anchor charts).
~AtlasStateSpace() override
Destructor.
AtlasChart * sampleChart() const
Pick a chart at random.
double estimateFrontierPercent() const
Estimate what percentage of atlas charts do not have fully formed polytope boundaries,...
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 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().
AtlasStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint, bool separate=true)
Construct an atlas with the specified dimensions.
AtlasChart * newChart(const StateType *state) const
Create a new chart for the atlas, centered at xorigin, which should be on the manifold....
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 StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
virtual void clear()
Clear any allocated memory from the state space.
A shared pointer wrapper for ompl::base::Constraint.
Abstract definition of a state space sampler.
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66