Loading...
Searching...
No Matches
BundleSpaceGraph.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18 * of its contributors may be used to endorse or promote products
19 * derived from this software without specific prior written
20 * permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andreas Orthey */
37
38#include <ompl/multilevel/datastructures/PlannerDataVertexAnnotated.h>
39#include <ompl/multilevel/datastructures/BundleSpaceGraph.h>
40#include <ompl/multilevel/datastructures/src/BundleSpaceGraphGoalVisitor.hpp>
41#include <ompl/multilevel/datastructures/Projection.h>
42
43#include <ompl/multilevel/datastructures/graphsampler/RandomVertex.h>
44#include <ompl/multilevel/datastructures/graphsampler/RandomDegreeVertex.h>
45#include <ompl/multilevel/datastructures/graphsampler/RandomEdge.h>
46#include <ompl/multilevel/datastructures/importance/Greedy.h>
47#include <ompl/multilevel/datastructures/importance/Exponential.h>
48#include <ompl/multilevel/datastructures/importance/Uniform.h>
49#include <ompl/multilevel/datastructures/metrics/Geodesic.h>
50#include <ompl/multilevel/datastructures/propagators/Geometric.h>
51#include <ompl/multilevel/datastructures/pathrestriction/PathRestriction.h>
52
53#include <ompl/geometric/PathSimplifier.h>
54#include <ompl/base/goals/GoalSampleableRegion.h>
55#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
56#include <ompl/base/objectives/MaximizeMinClearanceObjective.h>
57#include <ompl/datastructures/PDF.h>
58#include <ompl/tools/config/SelfConfig.h>
59#include <ompl/tools/config/MagicConstants.h>
60#include <ompl/util/Exception.h>
61#include <ompl/control/SpaceInformation.h>
62
63#include <ompl/datastructures/NearestNeighborsSqrtApprox.h>
64
65#include <boost/graph/astar_search.hpp>
66#include <boost/graph/incremental_components.hpp>
67#include <boost/graph/graphviz.hpp>
68#include <boost/property_map/vector_property_map.hpp>
69#include <boost/property_map/transform_value_property_map.hpp>
70
71#include <boost/foreach.hpp>
72
73#define foreach BOOST_FOREACH
74
76
77using namespace ompl::multilevel;
78
79BundleSpaceGraph::BundleSpaceGraph(const ompl::base::SpaceInformationPtr &si, BundleSpace *parent_) : BaseT(si, parent_)
80{
81 setName("BundleSpaceGraph");
82
83 // Functional primitives
84 setMetric("geodesic");
85 setGraphSampler("randomvertex");
86 setImportance("uniform");
87 setFindSectionStrategy(FindSectionType::SIDE_STEP);
88
89 if (isDynamic())
90 {
91 setPropagator("dynamic");
92 }
93 else
94 {
95 setPropagator("geometric");
96 }
97
98 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
99 specs_.approximateSolutions = false;
100 specs_.optimizingPaths = false;
101
102 Planner::declareParam<double>("range", this, &BundleSpaceGraph::setRange, &BundleSpaceGraph::getRange, "0.:1.:"
103 "10000.");
104
105 Planner::declareParam<double>("goal_bias", this, &BundleSpaceGraph::setGoalBias, &BundleSpaceGraph::getGoalBias,
106 "0.:.1:1.");
107
108 xRandom_ = new Configuration(getBundle());
109}
110
111BundleSpaceGraph::~BundleSpaceGraph()
112{
113 deleteConfiguration(xRandom_);
114}
115
117{
118 BaseT::setup();
119
122
123 OMPL_DEBUG("Range distance graph sampling: %f (max extent %f)", maxDistance_, getBundle()->getMaximumExtent());
124
126 {
127 if (!isDynamic())
128 {
130 }
131 else
132 {
133 // for dynamical systems, we use a quasimetric
134 //(non-symmetric metric), so we cannot use NN structures like GNAT
136 }
137 nearestDatastructure_->setDistanceFunction(
138 [this](const Configuration *a, const Configuration *b) { return distance(a, b); });
139 }
140
141 if (hasBaseSpace() && getProjection()->isFibered())
142 {
143 pathRestriction_ = std::make_shared<PathRestriction>(this);
144 }
145
146 if (pdef_)
147 {
148 setup_ = true;
149
150 optimizer_ = std::make_shared<ompl::geometric::PathSimplifier>(getBundle(), pdef_->getGoal(),
151 getOptimizationObjectivePtr());
152 optimizer_->freeStates(false);
153 }
154 else
155 {
156 setup_ = false;
157 }
158}
159
160void BundleSpaceGraph::setFindSectionStrategy(FindSectionType type)
161{
162 if (pathRestriction_ != nullptr)
163 {
164 pathRestriction_->setFindSectionStrategy(type);
165 }
166}
167
169{
170 if (hasBaseSpace() && getProjection()->isFibered())
171 {
172 base::PathPtr basePath = static_cast<BundleSpaceGraph *>(getChild())->getSolutionPathByReference();
173 pathRestriction_->setBasePath(basePath);
174
175 if (pathRestriction_->hasFeasibleSection(qStart_, qGoal_))
176 {
177 if (sameComponent(vStart_, qGoal_->index))
178 {
179 hasSolution_ = true;
180 return true;
181 }
182 }
183 }
184 return false;
185}
186
188{
189 BaseT::clear();
190
191 clearVertices();
192 pis_.restart();
193
194 graphLength_ = 0;
195 bestCost_ = base::Cost(base::dInf);
196 setup_ = false;
197 vStart_ = 0;
198 shortestVertexPath_.clear();
199
200 startConfigurations_.clear();
201 goalConfigurations_.clear();
202
203 if (!isDynamic())
204 {
205 if (solutionPath_ != nullptr)
206 {
207 std::static_pointer_cast<geometric::PathGeometric>(solutionPath_)->clear();
208 }
209 }
210
211 numVerticesWhenComputingSolutionPath_ = 0;
212
213 importanceCalculator_->clear();
214 graphSampler_->clear();
215 if (pathRestriction_ != nullptr)
216 {
217 pathRestriction_->clear();
218 }
219}
220
221void BundleSpaceGraph::clearVertices()
222{
224 {
225 std::vector<Configuration *> configs;
226 nearestDatastructure_->list(configs);
227 for (auto &config : configs)
228 {
229 deleteConfiguration(config);
230 }
231 nearestDatastructure_->clear();
232 }
233 graph_.clear();
234}
235
236void BundleSpaceGraph::setGoalBias(double goalBias)
237{
238 goalBias_ = goalBias;
239}
240
241double BundleSpaceGraph::getGoalBias() const
242{
243 return goalBias_;
244}
245
246void BundleSpaceGraph::setRange(double maxDistance)
247{
248 maxDistance_ = maxDistance;
249}
250
251double BundleSpaceGraph::getRange() const
252{
253 return maxDistance_;
254}
255
256BundleSpaceGraph::Configuration::Configuration(const ompl::base::SpaceInformationPtr &si) : state(si->allocState())
257{
258}
259BundleSpaceGraph::Configuration::Configuration(const ompl::base::SpaceInformationPtr &si,
260 const ompl::base::State *state_)
261 : state(si->cloneState(state_))
262{
263}
264
265void BundleSpaceGraph::deleteConfiguration(Configuration *q)
266{
267 if (q != nullptr)
268 {
269 if (q->state != nullptr)
270 {
271 getBundle()->freeState(q->state);
272 }
273 for (unsigned int k = 0; k < q->reachableSet.size(); k++)
274 {
275 Configuration *qk = q->reachableSet.at(k);
276 if (qk->state != nullptr)
277 {
278 getBundle()->freeState(qk->state);
279 }
280 }
281 if (isDynamic())
282 {
283 const ompl::control::SpaceInformationPtr siC =
284 std::static_pointer_cast<ompl::control::SpaceInformation>(getBundle());
285 siC->freeControl(q->control);
286 }
287 q->reachableSet.clear();
288
289 delete q;
290 q = nullptr;
291 }
292}
293
295{
296 return importanceCalculator_->eval();
297}
298
300{
301 if (const base::State *state = pis_.nextStart())
302 {
303 qStart_ = new Configuration(getBundle(), state);
304 vStart_ = addConfiguration(qStart_);
305 qStart_->isStart = true;
306 }
307
308 if (qStart_ == nullptr)
309 {
310 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
311 throw ompl::Exception("Invalid initial states.");
312 }
313
314 if (const base::State *state = pis_.nextGoal())
315 {
316 qGoal_ = new Configuration(getBundle(), state);
317 qGoal_->isGoal = true;
318 }
319
320 if (qGoal_ == nullptr && getGoalPtr()->canSample())
321 {
322 OMPL_ERROR("%s: There are no valid goal states!", getName().c_str());
323 throw ompl::Exception("Invalid goal states.");
324 }
325}
326
327void BundleSpaceGraph::uniteComponents(Vertex m1, Vertex m2)
328{
329 disjointSets_.union_set(m1, m2);
330}
331
332bool BundleSpaceGraph::sameComponent(Vertex m1, Vertex m2)
333{
334 return boost::same_component(m1, m2, disjointSets_);
335}
336
337const BundleSpaceGraph::Configuration *BundleSpaceGraph::nearest(const Configuration *q) const
338{
339 return nearestDatastructure_->nearest(const_cast<Configuration *>(q));
340}
341
348
350{
351 addEdge(a->index, b->index);
352}
353
355{
356 Vertex m = boost::add_vertex(q, graph_);
357 graph_[m]->total_connection_attempts = 1;
358 graph_[m]->successful_connection_attempts = 0;
359 disjointSets_.make_set(m);
360
361 nearestDatastructure_->add(q);
362 q->index = m;
363
364 return m;
365}
366
367unsigned int BundleSpaceGraph::getNumberOfVertices() const
368{
369 return num_vertices(graph_);
370}
371
372unsigned int BundleSpaceGraph::getNumberOfEdges() const
373{
374 return num_edges(graph_);
375}
376
378{
379 return graph_;
380}
381
386
387const BundleSpaceGraph::RoadmapNeighborsPtr &BundleSpaceGraph::getRoadmapNeighborsPtr() const
388{
390}
391
392ompl::base::Cost BundleSpaceGraph::costHeuristic(Vertex u, Vertex v) const
393{
394 return getOptimizationObjectivePtr()->motionCostHeuristic(graph_[u]->state, graph_[v]->state);
395}
396
397template <template <typename T> class NN>
398void BundleSpaceGraph::setNearestNeighbors()
399{
401 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
402 clear();
403 nearestDatastructure_ = std::make_shared<NN<base::State *>>();
404 if (!isSetup())
405 {
406 setup();
407 }
408}
409
411{
412 return metric_->distanceBundle(a, b);
413}
414
416{
417 return getBundle()->checkMotion(a->state, b->state);
418}
419
421{
422 metric_->interpolateBundle(a, b, dest);
423}
424
426{
427 Configuration *next = new Configuration(getBundle(), to->state);
428
429 if (!propagator_->steer(from, to, next))
430 {
431 deleteConfiguration(next);
432 return nullptr;
433 }
434 return next;
435}
436
438{
439 double d = distance(from, to);
440 if (d > maxDistance_)
441 {
442 metric_->interpolateBundle(from, to, maxDistance_ / d, to);
443 }
444
445 if (!propagator_->steer(from, to, to))
446 {
447 return nullptr;
448 }
449 Configuration *next = new Configuration(getBundle(), to->state);
450 return next;
451}
452
454{
455 if (!isDynamic())
456 {
457 double d = distance(from, to);
458 if (d > maxDistance_)
459 {
460 metric_->interpolateBundle(from, to, maxDistance_ / d, to);
461 }
462 }
463
464 if (!propagator_->steer(from, to, to))
465 {
466 return nullptr;
467 }
468
469 Configuration *next = new Configuration(getBundle(), to->state);
470 addConfiguration(next);
471 addBundleEdge(from, next);
472 return next;
473}
474
476{
477 if (!propagator_->steer(from, to, xRandom_))
478 {
479 return false;
480 }
481
482 addBundleEdge(from, to);
483 return true;
484}
485
486void BundleSpaceGraph::setPropagator(const std::string &sPropagator)
487{
488 if (sPropagator == "geometric")
489 {
490 OMPL_DEBUG("Geometric Propagator Selected");
491 propagator_ = std::make_shared<BundleSpacePropagatorGeometric>(this);
492 }
493 else
494 {
495 OMPL_ERROR("Propagator unknown: %s", sPropagator.c_str());
496 throw ompl::Exception("Unknown Propagator");
497 }
498}
499
500void BundleSpaceGraph::setMetric(const std::string &sMetric)
501{
502 if (sMetric == "geodesic")
503 {
504 OMPL_DEBUG("Geodesic Metric Selected");
505 metric_ = std::make_shared<BundleSpaceMetricGeodesic>(this);
506 }
507 else
508 {
509 OMPL_ERROR("Metric unknown: %s", sMetric.c_str());
510 throw ompl::Exception("Unknown Metric");
511 }
512}
513
514void BundleSpaceGraph::setImportance(const std::string &sImportance)
515{
516 if (sImportance == "uniform")
517 {
518 OMPL_DEBUG("Uniform Importance Selected");
519 importanceCalculator_ = std::make_shared<BundleSpaceImportanceUniform>(this);
520 }
521 else if (sImportance == "greedy")
522 {
523 OMPL_DEBUG("Greedy Importance Selected");
524 importanceCalculator_ = std::make_shared<BundleSpaceImportanceGreedy>(this);
525 }
526 else if (sImportance == "exponential")
527 {
528 OMPL_DEBUG("Greedy Importance Selected");
529 importanceCalculator_ = std::make_shared<BundleSpaceImportanceExponential>(this);
530 }
531 else
532 {
533 OMPL_ERROR("Importance calculator unknown: %s", sImportance.c_str());
534 throw ompl::Exception("Unknown Importance");
535 }
536}
537
538void BundleSpaceGraph::setGraphSampler(const std::string &sGraphSampler)
539{
540 if (sGraphSampler == "randomvertex")
541 {
542 OMPL_DEBUG("Random Vertex Sampler Selected");
543 graphSampler_ = std::make_shared<BundleSpaceGraphSamplerRandomVertex>(this);
544 }
545 else if (sGraphSampler == "randomedge")
546 {
547 OMPL_DEBUG("Random Edge Sampler Selected");
548 graphSampler_ = std::make_shared<BundleSpaceGraphSamplerRandomEdge>(this);
549 }
550 else if (sGraphSampler == "randomdegreevertex")
551 {
552 OMPL_DEBUG("Random Degree Vertex Sampler Selected");
553 graphSampler_ = std::make_shared<BundleSpaceGraphSamplerRandomDegreeVertex>(this);
554 }
555 else
556 {
557 OMPL_ERROR("Sampler unknown: %s", sGraphSampler.c_str());
558 throw ompl::Exception("Unknown Graph Sampler");
559 }
560}
561
562BundleSpaceGraphSamplerPtr BundleSpaceGraph::getGraphSampler()
563{
564 return graphSampler_;
565}
566
567const std::pair<BundleSpaceGraph::Edge, bool> BundleSpaceGraph::addEdge(const Vertex a, const Vertex b)
568{
569 base::Cost weight = getOptimizationObjectivePtr()->motionCost(graph_[a]->state, graph_[b]->state);
570 EdgeInternalState properties(weight);
571 const std::pair<Edge, bool> e = boost::add_edge(a, b, properties, graph_);
572 uniteComponents(a, b);
573 return e;
574}
575
576BundleSpaceGraph::Vertex BundleSpaceGraph::getStartIndex() const
577{
578 return vStart_;
579}
580
582{
583 goalConfigurations_.push_back(x);
584 if (getOptimizationObjectivePtr()->isCostBetterThan(x->cost, bestCost_))
585 {
586 bestCost_ = x->cost;
587 }
588}
589
590BundleSpaceGraph::Vertex BundleSpaceGraph::getGoalIndex() const
591{
592 if (goalConfigurations_.size() > 0)
593 {
594 return goalConfigurations_.front()->index;
595 }
596 else
597 {
598 OMPL_DEVMSG1("Returned NullVertex");
599 return nullVertex();
600 }
601}
602
603BundleSpaceGraph::Vertex BundleSpaceGraph::nullVertex() const
604{
605 return BGT::null_vertex();
606}
607
609{
610 vStart_ = idx;
611}
612
613ompl::base::PathPtr &BundleSpaceGraph::getSolutionPathByReference()
614{
615 return solutionPath_;
616}
617
618bool BundleSpaceGraph::getSolution(ompl::base::PathPtr &solution)
619{
620 if (hasSolution_)
621 {
622 if ((solutionPath_ != nullptr) && (getNumberOfVertices() == numVerticesWhenComputingSolutionPath_))
623 {
624 }
625 else
626 {
627 Vertex goalVertex;
628 for (unsigned int k = 0; k < goalConfigurations_.size(); k++)
629 {
631 if (sameComponent(vStart_, qk->index))
632 {
633 solutionPath_ = getPath(vStart_, qk->index);
634 goalVertex = qk->index;
635 break;
636 }
637 }
638 if (solutionPath_ == nullptr)
639 {
640 throw "hasSolution_ is set, but no solution exists.";
641 }
642 numVerticesWhenComputingSolutionPath_ = getNumberOfVertices();
643
644 if (!isDynamic() && solutionPath_ != solution && hasParent())
645 {
646 // @NOTE: optimization seems to improve feasibility of sections
647 // in low-dim problems (up to 20 dof roughly), but will take too
648 // much time for high-dim problems. Reducing vertices seems to
649 // be the only optimization not significantly slowing everything
650 // down.
651
652 bool valid = false;
653 for (unsigned int k = 0; k < 3; k++)
654 {
655 geometric::PathGeometric &gpath = static_cast<geometric::PathGeometric &>(*solutionPath_);
656
657 valid = optimizer_->reduceVertices(gpath, 0, 0, 0.1);
658
659 if (!valid)
660 {
661 // reset solutionPath
662 solutionPath_ = getPath(vStart_, goalVertex);
663 }
664 else
665 {
666 break;
667 }
668 }
669 }
670 }
671 solution = solutionPath_;
672 return true;
673 }
674 else
675 {
676 return false;
677 }
678}
679
680ompl::base::PathPtr BundleSpaceGraph::getPath(const Vertex &start, const Vertex &goal)
681{
682 return getPath(start, goal, graph_);
683}
684
685ompl::base::PathPtr BundleSpaceGraph::getPath(const Vertex &start, const Vertex &goal, Graph &graph)
686{
687 std::vector<Vertex> prev(boost::num_vertices(graph));
688 auto weight = boost::make_transform_value_property_map(std::mem_fn(&EdgeInternalState::getCost),
689 get(boost::edge_bundle, graph));
690
691 try
692 {
693 boost::astar_search(graph, start, [this, goal](const Vertex v) { return costHeuristic(v, goal); },
694 boost::predecessor_map(&prev[0])
695 .weight_map(weight)
696 .distance_compare([this](EdgeInternalState c1, EdgeInternalState c2) {
697 return getOptimizationObjectivePtr()->isCostBetterThan(c1.getCost(), c2.getCost());
698 })
699 .distance_combine([this](EdgeInternalState c1, EdgeInternalState c2) {
700 return getOptimizationObjectivePtr()->combineCosts(c1.getCost(), c2.getCost());
701 })
702 .distance_inf(getOptimizationObjectivePtr()->infiniteCost())
703 .distance_zero(getOptimizationObjectivePtr()->identityCost())
704 .visitor(BundleSpaceGraphGoalVisitor<Vertex>(goal)));
705 }
706 catch (BundleSpaceGraphFoundGoal &)
707 {
708 }
709
710 auto p(std::make_shared<geometric::PathGeometric>(getBundle()));
711 if (prev[goal] == goal)
712 {
713 return nullptr;
714 }
715
716 std::vector<Vertex> vpath;
717 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
718 {
719 graph[pos]->on_shortest_path = true;
720 vpath.push_back(pos);
721 p->append(graph[pos]->state);
722 }
723 graph[start]->on_shortest_path = true;
724 vpath.push_back(start);
725 p->append(graph[start]->state);
726
727 shortestVertexPath_.clear();
728 shortestVertexPath_.insert(shortestVertexPath_.begin(), vpath.rbegin(), vpath.rend());
729 p->reverse();
730
731 return p;
732}
733
734void BundleSpaceGraph::sampleBundleGoalBias(ompl::base::State *xRandom)
735{
736 if (hasSolution_)
737 {
738 // No Goal Biasing if we already found a solution on this bundle space
739 sampleBundle(xRandom);
740 }
741 else
742 {
743 double s = rng_.uniform01();
744 if (s < goalBias_ && getGoalPtr()->canSample())
745 {
746 getGoalPtr()->sampleGoal(xRandom);
747 }
748 else
749 {
750 sampleBundle(xRandom);
751 }
752 }
753}
754
755void BundleSpaceGraph::sampleFromDatastructure(ompl::base::State *xRandom)
756{
757 graphSampler_->sample(xRandom);
758}
759
760void BundleSpaceGraph::writeToGraphviz(std::string filename) const
761{
762 std::ofstream f(filename.c_str());
763 std::vector<std::string> annotationVec;
764 foreach (const Vertex v, boost::vertices(graph_))
765 {
766 Configuration *qv = graph_[v];
767 const base::State *s = qv->state;
768 std::ostringstream out;
769 getBundle()->printState(s, out);
770 annotationVec.push_back(out.str());
771 }
772 write_graphviz(f, graph_, boost::make_label_writer(&annotationVec[0]));
773}
774
775void BundleSpaceGraph::print(std::ostream &out) const
776{
777 BaseT::print(out);
778 out << std::endl
779 << " --[BundleSpaceGraph has " << getNumberOfVertices() << " vertices and " << getNumberOfEdges() << " edges.]"
780 << std::endl;
781}
782
784{
785 getBundle()->printState(q->state);
786}
787
788void BundleSpaceGraph::getPlannerDataGraph(ompl::base::PlannerData &data, const Graph &graph, const Vertex vStart) const
789{
790 if (boost::num_vertices(graph) <= 0)
791 return;
792
793 multilevel::PlannerDataVertexAnnotated pstart(graph[vStart]->state);
794 pstart.setLevel(getLevel());
795 data.addStartVertex(pstart);
796
797 for (unsigned int k = 0; k < goalConfigurations_.size(); k++)
798 {
799 Configuration *qgoal = goalConfigurations_.at(k);
800 multilevel::PlannerDataVertexAnnotated pgoal(qgoal->state);
801 pgoal.setLevel(getLevel());
802 data.addGoalVertex(pgoal);
803 }
804 if (hasSolution_)
805 {
806 if (solutionPath_ != nullptr)
807 {
808 geometric::PathGeometric &gpath = static_cast<geometric::PathGeometric &>(*solutionPath_);
809
810 std::vector<base::State *> gstates = gpath.getStates();
811
813
814 for (unsigned int k = 1; k < gstates.size(); k++)
815 {
817 p.setLevel(getLevel());
818 data.addVertex(p);
819 data.addEdge(*pLast, p);
820 pLast = &p;
821 }
822 }
823 }
824
825 foreach (const Edge e, boost::edges(graph))
826 {
827 const Vertex v1 = boost::source(e, graph);
828 const Vertex v2 = boost::target(e, graph);
829
830 multilevel::PlannerDataVertexAnnotated p1(graph[v1]->state);
831 multilevel::PlannerDataVertexAnnotated p2(graph[v2]->state);
832 p1.setLevel(getLevel());
833 p2.setLevel(getLevel());
834 data.addEdge(p1, p2);
835 }
836 foreach (const Vertex v, boost::vertices(graph))
837 {
838 multilevel::PlannerDataVertexAnnotated p(graph[v]->state);
839 p.setLevel(getLevel());
840 data.addVertex(p);
841 }
842}
843
845{
846 OMPL_DEBUG("Graph (level %d) has %d/%d vertices/edges", getLevel(), boost::num_vertices(graph_),
847 boost::num_edges(graph_));
848
849 if (bestCost_.value() < ompl::base::dInf)
850 {
851 OMPL_DEBUG("Best Cost: %.2f", bestCost_.value());
852 }
853 getPlannerDataGraph(data, graph_, vStart_);
854}
The exception type for ompl.
Definition Exception.h:47
A nearest neighbors datastructure that uses linear search. The linear search is done over sqrt(n) ele...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
bool isSetup() const
Check if setup() was called for this planner.
Definition Planner.cpp:113
PlannerInputStates pis_
Utility class to extract valid input states.
Definition Planner.h:416
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
bool setup_
Flag indicating whether setup() has been called.
Definition Planner.h:433
Definition of an abstract state.
Definition State.h:50
Definition of a geometric path.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
bool connect(const Configuration *from, const Configuration *to)
Try to connect configuration a to configuration b using the current metric.
BundleSpaceGraphSamplerPtr graphSampler_
Pointer to strategy to sample from graph.
bool getSolution(ompl::base::PathPtr &solution) override
Return best solution.
BundleSpaceImportancePtr importanceCalculator_
Pointer to strategy to compute importance of this bundle space (which is used to decide which bundle ...
PathRestrictionPtr pathRestriction_
Pointer to current path restriction (the set of points which project onto the best cost path on the b...
Configuration * extendGraphTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to while system is valid, stopping if maxDistan...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
double maxDistance_
Maximum expansion step.
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
RoadmapNeighborsPtr nearestDatastructure_
Nearest neighbor structure for Bundle space configurations.
Configuration * qGoal_
The (best) goal configuration.
void writeToGraphviz(std::string filename) const
Write class to graphviz.
Configuration * xRandom_
Temporary random configuration.
double getImportance() const override
Importance of Bundle-space depending on number of vertices in Bundle-graph.
Configuration * steerTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to, stopping if maxdistance is reached.
virtual const std::pair< Edge, bool > addEdge(const Vertex a, const Vertex b)
Add edge between Vertex a and Vertex b to graph.
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on Bundle-graph.
virtual void setStartIndex(Vertex)
Set vertex representing the start.
Configuration * steerTowards(const Configuration *from, const Configuration *to)
Steer system at Configuration *from to Configuration *to.
std::vector< Configuration * > goalConfigurations_
List of configurations that satisfy the goal condition.
base::Cost bestCost_
Best cost found so far by algorithm.
virtual void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
std::vector< Configuration * > startConfigurations_
List of configurations that satisfy the start condition.
virtual Vertex getStartIndex() const
Get vertex representing the start.
virtual void interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const
Interpolate from configuration a to configuration b and store results in dest.
virtual Configuration * addBundleConfiguration(base::State *)
Add ompl::base::State to graph. Return its configuration.
virtual void printConfiguration(const Configuration *) const
Print configuration to std::cout.
virtual Graph & getGraphNonConst()
Get underlying boost graph representation (non const)
virtual void addBundleEdge(const Configuration *a, const Configuration *b)
Add edge between configuration a and configuration b to graph.
virtual double distance(const Configuration *a, const Configuration *b) const
Distance between two configurations using the current metric.
virtual bool checkMotion(const Configuration *a, const Configuration *b) const
Check if we can move from configuration a to configuration b using the current metric.
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphMetaData > Graph
A Bundle-graph structure using boost::adjacency_list bundles.
virtual const Graph & getGraph() const
Get underlying boost graph representation.
void addGoalConfiguration(Configuration *x)
Add configuration to graph as goal vertex.
void print(std::ostream &out) const override
Print class to ostream.
ompl::geometric::PathSimplifierPtr optimizer_
A path optimizer.
bool findSection() override
Call algorithm to solve the find section problem.
Configuration * qStart_
Start configuration.
void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
double graphLength_
Length of graph (useful for determing importance of Bundle-space.
virtual Vertex getGoalIndex() const
Get vertex representing the goal.
A single Bundle-space.
Definition BundleSpace.h:64
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
bool hasParent() const
Return if has parent space pointer.
BundleSpacePropagatorPtr propagator_
Propagator (steering or interpolation) on bundle space. Note: currently just a stub for base::StatePr...
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
bool hasSolution_
If there exists a solution.
BundleSpaceMetricPtr metric_
Metric on bundle space.
BundleSpace * getChild() const
Return k-1 th bundle space (locally the base space)
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
bool hasBaseSpace() const
Return if has base space pointer.
virtual void print(std::ostream &out) const
Internal function implementing actual printing to stream.
unsigned int getLevel() const
Level in hierarchy of Bundle-spaces.
An annotated vertex, adding information about its level in the multilevel hierarchy....
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
This namespace contains datastructures and planners to exploit multilevel abstractions,...