37 #include "ompl/base/spaces/constraint/AtlasStateSpace.h"
38 #include "ompl/base/spaces/constraint/AtlasChart.h"
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/util/Exception.h"
55 const std::size_t k = atlas_->getManifoldDimension();
56 Eigen::VectorXd ru(k);
61 unsigned int tries = ompl::magic::ATLAS_STATE_SPACE_SAMPLES;
68 c = atlas_->sampleChart();
73 for (std::size_t i = 0; i < k; ++i)
74 ru[i] = rng_.gaussian01();
76 ru *= atlas_->getRho_s() * std::pow(rng_.uniform01(), 1.0 / k) / ru.norm();
77 }
while (tries-- > 0 && !c->inPolytope(ru));
80 }
while (tries > 0 && !c->psi(ru, *astate));
86 OMPL_WARN(
"ompl::base::AtlasStateSpace::sampleUniform(): "
87 "Took too long; returning center of a random chart.");
88 atlas_->copyState(astate, c->getOrigin());
91 space_->enforceBounds(state);
94 c->psiInverse(*astate, ru);
96 astate->setChart(atlas_->owningChart(astate));
105 const std::size_t k = atlas_->getManifoldDimension();
107 Eigen::VectorXd ru(k), uoffset(k);
109 AtlasChart *c = atlas_->getChart(anear,
true);
112 OMPL_ERROR(
"ompl::base::AtlasStateSpace::sampleUniformNear(): "
113 "Sampling failed because chart creation failed! Falling back to uniform sample.");
114 sampleUniform(state);
119 c->psiInverse(*anear, ru);
120 unsigned int tries = ompl::magic::ATLAS_STATE_SPACE_SAMPLES;
124 for (std::size_t i = 0; i < k; ++i)
125 uoffset[i] = ru[i] + rng_.gaussian01();
127 uoffset *= dist * std::pow(rng_.uniform01(), 1.0 / k) / uoffset.norm();
128 }
while (--tries > 0 && !c->psi(uoffset, *astate));
134 OMPL_WARN(
"ompl::base:::AtlasStateSpace::sampleUniformNear(): "
135 "Took too long; returning initial point.");
136 atlas_->copyState(state, near);
139 space_->enforceBounds(state);
141 c->psiInverse(*astate, ru);
142 if (!c->inPolytope(ru))
143 c = atlas_->getChart(astate,
true);
155 const std::size_t k = atlas_->getManifoldDimension();
156 Eigen::VectorXd ru(k), rand(k);
158 AtlasChart *c = atlas_->getChart(amean,
true);
161 OMPL_ERROR(
"ompl::base::AtlasStateSpace::sampleGaussian(): "
162 "Sampling failed because chart creation failed! Falling back to uniform sample.");
163 sampleUniform(state);
167 c->psiInverse(*amean, ru);
170 unsigned int tries = ompl::magic::ATLAS_STATE_SPACE_SAMPLES;
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));
180 OMPL_WARN(
"ompl::base::AtlasStateSpace::sampleUniforGaussian(): "
181 "Took too long; returning initial point.");
182 atlas_->copyState(state, mean);
185 space_->enforceBounds(state);
187 c->psiInverse(*astate, ru);
188 if (!c->inPolytope(ru))
189 c = atlas_->getChart(astate,
true);
203 , biasFunction_([](
AtlasChart *) -> double { return 1; })
204 , separate_(separate)
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);
210 setName(
"Atlas" + space_->getName());
212 chartNN_.setDistanceFunction(
213 [&](
const NNElement &e1,
const NNElement &e2) ->
double {
return distance(e1.first, e2.first); });
219 for (
auto anchor : anchors_)
229 for (
auto chart : charts_)
233 std::vector<NNElement> nnList;
234 chartNN_.list(nnList);
235 for (
auto &chart : nnList)
237 const State *state = chart.first;
238 freeState(
const_cast<State *
>(state));
245 for (
auto anchor : anchors_)
253 auto anchor = cloneState(state)->as<
StateType>();
254 anchors_.push_back(anchor);
258 if (chart ==
nullptr)
260 "Initial chart creation failed. Cannot proceed.");
277 OMPL_ERROR(
"ompl::base::AtlasStateSpace::newChart(): "
278 "Failed because manifold looks degenerate here.");
280 if (cstate !=
nullptr)
290 std::vector<NNElement> nearbyCharts;
291 chartNN_.nearestR(std::make_pair(cstate, 0), 2 * rho_s_, nearbyCharts);
293 for (
auto &&near : nearbyCharts)
298 chartPDF_.update(chartPDF_.getElements()[near.second], biasFunction_(other));
302 chartNN_.add(std::make_pair(cstate, charts_.size()));
303 charts_.push_back(chart);
304 chartPDF_.add(chart, biasFunction_(chart));
313 "Atlas sampled before any charts were made. Use AtlasStateSpace::anchorChart() first.");
315 return chartPDF_.sample(rng_.uniform01());
321 if (c ==
nullptr || force)
323 c = owningChart(state);
328 if (created !=
nullptr)
341 Eigen::VectorXd u_t(k_);
342 auto temp = allocState()->as<
StateType>();
344 std::vector<NNElement> nearby;
345 chartNN_.nearestR(std::make_pair(state, 0), rho_, nearby);
347 double best = epsilon_;
349 for (
auto & near : nearby)
352 auto owner = charts_[near.second];
354 owner->phi(u_t, *temp);
357 if (owner->inPolytope(u_t)
358 && (far = distance(state, temp)) < epsilon_
371 std::vector<ompl::base::State *> *geodesic)
const
373 auto &&svc = si_->getStateValidityChecker();
376 if (!constraint_->isSatisfied(from) || !(interpolate || svc->isValid(from)))
388 if (geodesic !=
nullptr)
391 geodesic->push_back(cloneState(from));
395 const double tolerance = delta_;
396 const double distTo = distance(from, to);
397 if (distTo <= tolerance)
401 const double distMax = lambda_ * distTo;
404 auto scratch = cloneState(from)->as<
StateType>();
408 Eigen::VectorXd u_j(k_), u_b(k_);
409 c->psiInverse(*scratch, u_j);
410 c->psiInverse(*ato, u_b);
413 std::size_t chartsCreated = 0;
424 u_j += factor * delta_ * (u_b - u_j).normalized();
427 const bool onManifold = c->psi(u_j, *temp);
434 const double step = distance(scratch, temp);
436 if (step < std::numeric_limits<double>::epsilon())
439 const bool exceedStepSize = step >= lambda_ * delta_;
449 copyState(scratch, temp);
450 scratch->setChart(c);
452 if (!(interpolate || svc->isValid(scratch))
453 || distance(from, scratch) > distMax
455 || chartsCreated > maxChartsPerExtension_)
465 if (distance(scratch, temp) > epsilon_
466 || delta_ / step < cos_alpha_
467 || !c->inPolytope(u_j))
469 bool created =
false;
470 if ((c = getChart(scratch,
true, &created)) ==
nullptr)
472 OMPL_ERROR(
"Treating singularity as an obstacle.");
476 chartsCreated += created;
479 c->psiInverse(*scratch, u_j);
480 c->psiInverse(*ato, u_b);
483 done = distance(scratch, to) <= delta_;
487 if (geodesic !=
nullptr)
488 geodesic->push_back(cloneState(scratch));
492 const bool ret = done && distance(to, scratch) <= delta_;
503 frontier += c->estimateIsFrontier() ? 1 : 0;
505 return (100 * frontier) / charts_.size();
510 std::stringstream v, f;
511 std::size_t vcount = 0;
512 std::size_t fcount = 0;
513 std::vector<Eigen::VectorXd> vertices;
517 c->toPolygon(vertices);
519 std::stringstream poly;
520 std::size_t fvcount = 0;
521 for (Eigen::VectorXd &vert : vertices)
523 v << vert.transpose() <<
"\n";
524 poly << vcount++ <<
" ";
530 f << fvcount <<
" " << poly.str() <<
"\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();
The exception type for ompl.
Tangent space and bounding polytope approximating some patch of the manifold.
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.
const T * as() const
Cast this instance to a desired type.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.