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
47ompl::base::AtlasStateSampler::AtlasStateSampler(const AtlasStateSpace *space) : StateSampler(space), atlas_(space)
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
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
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 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).
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.
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...
PDF< AtlasChart * > chartPDF_
PDF of charts according to a bias function.
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 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.
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.
RNG rng_
Random number generator.
AtlasChartBiasFunction biasFunction_
Function to bias chart sampling.
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....
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.
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...
virtual void clear()
Clear any allocated memory from the state space.
double lambda_
Manifold traversal from x to y is stopped if accumulated distance is greater than d(x,...
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...
double delta_
Step size when traversing the manifold and collision checking.
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
const unsigned int k_
Manifold dimension.
SpaceInformation * si_
SpaceInformation associated with this space. Required for early collision checking in manifold traver...
const ConstraintPtr constraint_
Constraint function that defines the manifold.
A shared pointer wrapper for ompl::base::Constraint.
const StateSpace * space_
The state space this sampler samples.
A shared pointer wrapper for ompl::base::StateSpace.
State * cloneState(const State *source) const
Clone a state.
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 freeState(State *state) const override
Free the memory of the allocated state.
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
#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