Loading...
Searching...
No Matches
SPARStwo.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
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 Rutgers 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: Andrew Dobson */
36
37#include "ompl/geometric/planners/prm/SPARStwo.h"
38#include "ompl/geometric/planners/prm/ConnectionStrategy.h"
39#include "ompl/base/goals/GoalSampleableRegion.h"
40#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
41#include "ompl/tools/config/SelfConfig.h"
42#include <boost/graph/astar_search.hpp>
43#include <boost/graph/incremental_components.hpp>
44#include <boost/property_map/vector_property_map.hpp>
45#include <boost/foreach.hpp>
46#include <thread>
47
48#include "GoalVisitor.hpp"
49
50#define foreach BOOST_FOREACH
51#define foreach_reverse BOOST_REVERSE_FOREACH
52
53ompl::geometric::SPARStwo::SPARStwo(const base::SpaceInformationPtr &si)
54 : base::Planner(si, "SPARStwo")
55 , nearSamplePoints_((2 * si_->getStateDimension()))
56 , stateProperty_(boost::get(vertex_state_t(), g_))
57 , weightProperty_(boost::get(boost::edge_weight, g_))
58 , colorProperty_(boost::get(vertex_color_t(), g_))
60 , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
61{
62 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
63 specs_.approximateSolutions = false;
64 specs_.optimizingPaths = true;
65 specs_.multithreaded = true;
66
67 psimp_ = std::make_shared<PathSimplifier>(si_);
68
69 Planner::declareParam<double>("stretch_factor", this, &SPARStwo::setStretchFactor, &SPARStwo::getStretchFactor,
70 "1.1:0.1:3.0");
71 Planner::declareParam<double>("sparse_delta_fraction", this, &SPARStwo::setSparseDeltaFraction,
72 &SPARStwo::getSparseDeltaFraction, "0.0:0.01:1.0");
73 Planner::declareParam<double>("dense_delta_fraction", this, &SPARStwo::setDenseDeltaFraction,
74 &SPARStwo::getDenseDeltaFraction, "0.0:0.0001:0.1");
75 Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures,
76 "100:10:3000");
77
78 addPlannerProgressProperty("iterations INTEGER", [this] { return getIterationCount(); });
79 addPlannerProgressProperty("best cost REAL", [this] { return getBestCost(); });
80}
81
86
88{
89 Planner::setup();
90 if (!nn_)
92 nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
93 double maxExt = si_->getMaximumExtent();
96
97 // Setup optimization objective
98 //
99 // If no optimization objective was specified, then default to
100 // optimizing path length as computed by the distance() function
101 // in the state space.
102 if (pdef_)
103 {
104 if (pdef_->hasOptimizationObjective())
105 {
106 opt_ = pdef_->getOptimizationObjective();
107 if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()) == nullptr)
108 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
109 "for other optimizaton objectives is not guaranteed.",
110 getName().c_str());
111 }
112 else
113 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
114 }
115 else
116 {
117 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
118 setup_ = false;
119 }
120}
121
122void ompl::geometric::SPARStwo::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
123{
124 Planner::setProblemDefinition(pdef);
125 clearQuery();
126}
127
129{
130 startM_.clear();
131 goalM_.clear();
132 pis_.restart();
133}
134
136{
137 Planner::clear();
138 clearQuery();
140 iterations_ = 0;
141 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
142 freeMemory();
143 if (nn_)
144 nn_->clear();
145}
146
148{
149 Planner::clear();
150 sampler_.reset();
151
152 foreach (Vertex v, boost::vertices(g_))
153 {
154 foreach (InterfaceData &d, interfaceDataProperty_[v] | boost::adaptors::map_values)
155 d.clear(si_);
156 if (stateProperty_[v] != nullptr)
157 si_->freeState(stateProperty_[v]);
158 stateProperty_[v] = nullptr;
159 }
160 g_.clear();
161
162 if (nn_)
163 nn_->clear();
164}
165
166bool ompl::geometric::SPARStwo::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
167 base::PathPtr &solution)
168{
169 base::Goal *g = pdef_->getGoal().get();
170 base::Cost sol_cost(opt_->infiniteCost());
171 foreach (Vertex start, starts)
172 foreach (Vertex goal, goals)
173 {
174 // we lock because the connected components algorithm is incremental and may change disjointSets_
175 graphMutex_.lock();
176 bool same_component = sameComponent(start, goal);
177 graphMutex_.unlock();
178
179 if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
180 {
181 base::PathPtr p = constructSolution(start, goal);
182 if (p)
183 {
184 base::Cost pathCost = p->cost(opt_);
185 if (opt_->isCostBetterThan(pathCost, bestCost_))
186 bestCost_ = pathCost;
187 // Check if optimization objective is satisfied
188 if (opt_->isSatisfied(pathCost))
189 {
190 solution = p;
191 return true;
192 }
193 if (opt_->isCostBetterThan(pathCost, sol_cost))
194 {
195 solution = p;
196 sol_cost = pathCost;
197 }
198 }
199 }
200 }
201 return false;
202}
203
205{
206 return boost::same_component(m1, m2, disjointSets_);
207}
208
213
218
220{
221 if (stopOnMaxFail)
222 {
224 base::PlannerTerminationCondition ptcOrFail([this, &ptc] { return ptc || reachedFailureLimit(); });
225 constructRoadmap(ptcOrFail);
226 }
227 else
228 constructRoadmap(ptc);
229}
230
232{
234
235 if (!isSetup())
236 setup();
237 if (!sampler_)
238 sampler_ = si_->allocValidStateSampler();
239
240 base::State *qNew = si_->allocState();
241 base::State *workState = si_->allocState();
242
243 /* The whole neighborhood set which has been most recently computed */
244 std::vector<Vertex> graphNeighborhood;
245 /* The visible neighborhood set which has been most recently computed */
246 std::vector<Vertex> visibleNeighborhood;
247
248 bestCost_ = opt_->infiniteCost();
249 while (!ptc)
250 {
251 ++iterations_;
253
254 // Generate a single sample, and attempt to connect it to nearest neighbors.
255 if (!sampler_->sample(qNew))
256 continue;
257
258 findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);
259
260 if (!checkAddCoverage(qNew, visibleNeighborhood))
261 if (!checkAddConnectivity(qNew, visibleNeighborhood))
262 if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
263 {
264 if (!visibleNeighborhood.empty())
265 {
266 std::map<Vertex, base::State *> closeRepresentatives;
267 findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
268 for (auto &closeRepresentative : closeRepresentatives)
269 {
270 updatePairPoints(visibleNeighborhood[0], qNew, closeRepresentative.first,
271 closeRepresentative.second);
272 updatePairPoints(closeRepresentative.first, closeRepresentative.second,
273 visibleNeighborhood[0], qNew);
274 }
275 checkAddPath(visibleNeighborhood[0]);
276 for (auto &closeRepresentative : closeRepresentatives)
277 {
278 checkAddPath(closeRepresentative.first);
279 si_->freeState(closeRepresentative.second);
280 }
281 }
282 }
283 }
284 si_->freeState(workState);
285 si_->freeState(qNew);
286}
287
289{
290 std::lock_guard<std::mutex> _(graphMutex_);
291 if (boost::num_vertices(g_) < 1)
292 {
293 queryVertex_ = boost::add_vertex(g_);
294 stateProperty_[queryVertex_] = nullptr;
295 }
296}
297
299{
302
303 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
304
305 if (goal == nullptr)
306 {
307 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
309 }
310
311 // Add the valid start states as milestones
312 while (const base::State *st = pis_.nextStart())
313 startM_.push_back(addGuard(si_->cloneState(st), START));
314 if (startM_.empty())
315 {
316 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
318 }
319
320 if (!goal->couldSample())
321 {
322 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
324 }
325
326 // Add the valid goal states as milestones
327 while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
328 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
329 if (goalM_.empty())
330 {
331 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
333 }
334
335 unsigned int nrStartStates = boost::num_vertices(g_) - 1; // don't count query vertex
336 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nrStartStates);
337
338 // Reset addedSolution_ member
339 addedSolution_ = false;
341 base::PathPtr sol;
342 base::PlannerTerminationCondition ptcOrFail([this, &ptc] { return ptc || reachedFailureLimit(); });
343 std::thread slnThread([this, &ptcOrFail, &sol] { checkForSolution(ptcOrFail, sol); });
344
345 // Construct planner termination condition which also takes M into account
346 base::PlannerTerminationCondition ptcOrStop([this, &ptc] { return ptc || reachedTerminationCriterion(); });
347 constructRoadmap(ptcOrStop);
348
349 // Ensure slnThread is ceased before exiting solve
350 slnThread.join();
351
352 OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
353
354 if (sol)
355 {
356 pdef_->addSolutionPath(sol, false, -1.0, getName());
358 }
359 else
360 {
362 }
363}
364
366{
367 auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
368 while (!ptc && !addedSolution_)
369 {
370 // Check for any new goal states
371 if (goal->maxSampleCount() > goalM_.size())
372 {
373 const base::State *st = pis_.nextGoal();
374 if (st != nullptr)
375 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
376 }
377
378 // Check for a solution
380 // Sleep for 1ms
381 if (!addedSolution_)
382 std::this_thread::sleep_for(std::chrono::milliseconds(1));
383 }
384}
385
386bool ompl::geometric::SPARStwo::checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
387{
388 if (!visibleNeighborhood.empty())
389 return false;
390 // No free paths means we add for coverage
391 addGuard(si_->cloneState(qNew), COVERAGE);
392 return true;
393}
394
395bool ompl::geometric::SPARStwo::checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
396{
397 std::vector<Vertex> links;
398 if (visibleNeighborhood.size() > 1)
399 {
400 // For each neighbor
401 for (std::size_t i = 0; i < visibleNeighborhood.size(); ++i)
402 // For each other neighbor
403 for (std::size_t j = i + 1; j < visibleNeighborhood.size(); ++j)
404 // If they are in different components
405 if (!sameComponent(visibleNeighborhood[i], visibleNeighborhood[j]))
406 {
407 links.push_back(visibleNeighborhood[i]);
408 links.push_back(visibleNeighborhood[j]);
409 }
410
411 if (!links.empty())
412 {
413 // Add the node
414 Vertex g = addGuard(si_->cloneState(qNew), CONNECTIVITY);
415
416 for (unsigned long link : links)
417 // If there's no edge
418 if (!boost::edge(g, link, g_).second)
419 // And the components haven't been united by previous links
420 if (!sameComponent(link, g))
421 connectGuards(g, link);
422 return true;
423 }
424 }
425 return false;
426}
427
428bool ompl::geometric::SPARStwo::checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
429 std::vector<Vertex> &visibleNeighborhood)
430{
431 // If we have more than 1 or 0 neighbors
432 if (visibleNeighborhood.size() > 1)
433 if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
434 // If our two closest neighbors don't share an edge
435 if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], g_).second)
436 {
437 // If they can be directly connected
438 if (si_->checkMotion(stateProperty_[visibleNeighborhood[0]], stateProperty_[visibleNeighborhood[1]]))
439 {
440 // Connect them
441 connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]);
442 // And report that we added to the roadmap
444 // Report success
445 return true;
446 }
447
448 // Add the new node to the graph, to bridge the interface
449 Vertex v = addGuard(si_->cloneState(qNew), INTERFACE);
450 connectGuards(v, visibleNeighborhood[0]);
451 connectGuards(v, visibleNeighborhood[1]);
452 // Report success
453 return true;
454 }
455 return false;
456}
457
459{
460 bool ret = false;
461
462 std::vector<Vertex> rs;
463 foreach (Vertex r, boost::adjacent_vertices(v, g_))
464 rs.push_back(r);
465
466 /* Candidate x vertices as described in the method, filled by function computeX(). */
467 std::vector<Vertex> Xs;
468
469 /* Candidate v" vertices as described in the method, filled by function computeVPP(). */
470 std::vector<Vertex> VPPs;
471
472 for (std::size_t i = 0; i < rs.size() && !ret; ++i)
473 {
474 Vertex r = rs[i];
475 computeVPP(v, r, VPPs);
476 foreach (Vertex rp, VPPs)
477 {
478 // First, compute the longest path through the graph
479 computeX(v, r, rp, Xs);
480 double rm_dist = 0.0;
481 foreach (Vertex rpp, Xs)
482 {
483 double tmp_dist = (si_->distance(stateProperty_[r], stateProperty_[v]) +
484 si_->distance(stateProperty_[v], stateProperty_[rpp])) /
485 2.0;
486 if (tmp_dist > rm_dist)
487 rm_dist = tmp_dist;
488 }
489
490 InterfaceData &d = getData(v, r, rp);
491
492 // Then, if the spanner property is violated
493 if (rm_dist > stretchFactor_ * d.d_)
494 {
495 ret = true; // Report that we added for the path
496 if (si_->checkMotion(stateProperty_[r], stateProperty_[rp]))
497 connectGuards(r, rp);
498 else
499 {
500 auto p(std::make_shared<PathGeometric>(si_));
501 if (r < rp)
502 {
503 p->append(d.sigmaA_);
504 p->append(d.pointA_);
505 p->append(stateProperty_[v]);
506 p->append(d.pointB_);
507 p->append(d.sigmaB_);
508 }
509 else
510 {
511 p->append(d.sigmaB_);
512 p->append(d.pointB_);
513 p->append(stateProperty_[v]);
514 p->append(d.pointA_);
515 p->append(d.sigmaA_);
516 }
517
518 psimp_->reduceVertices(*p, 10);
519 psimp_->shortcutPath(*p, 50);
520
521 if (p->checkAndRepair(100).second)
522 {
523 Vertex prior = r;
524 Vertex vnew;
525 std::vector<base::State *> &states = p->getStates();
526
527 foreach (base::State *st, states)
528 {
529 // no need to clone st, since we will destroy p; we just copy the pointer
530 vnew = addGuard(st, QUALITY);
531
532 connectGuards(prior, vnew);
533 prior = vnew;
534 }
535 // clear the states, so memory is not freed twice
536 states.clear();
537 connectGuards(prior, rp);
538 }
539 }
540 }
541 }
542 }
543
544 return ret;
545}
546
551
552void ompl::geometric::SPARStwo::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood,
553 std::vector<Vertex> &visibleNeighborhood)
554{
555 visibleNeighborhood.clear();
557 nn_->nearestR(queryVertex_, sparseDelta_, graphNeighborhood);
558 stateProperty_[queryVertex_] = nullptr;
559
560 // Now that we got the neighbors from the NN, we must remove any we can't see
561 for (unsigned long i : graphNeighborhood)
562 if (si_->checkMotion(st, stateProperty_[i]))
563 visibleNeighborhood.push_back(i);
564}
565
567{
568 std::vector<Vertex> hold;
569 nn_->nearestR(v, sparseDelta_, hold);
570
571 std::vector<Vertex> neigh;
572 for (unsigned long i : hold)
573 if (si_->checkMotion(stateProperty_[v], stateProperty_[i]))
574 neigh.push_back(i);
575
576 foreach (Vertex vp, neigh)
577 connectGuards(v, vp);
578}
579
581{
582 std::vector<Vertex> nbh;
584 nn_->nearestR(queryVertex_, sparseDelta_, nbh);
585 stateProperty_[queryVertex_] = nullptr;
586
587 Vertex result = boost::graph_traits<Graph>::null_vertex();
588
589 for (unsigned long i : nbh)
590 if (si_->checkMotion(st, stateProperty_[i]))
591 {
592 result = i;
593 break;
594 }
595 return result;
596}
597
599 const Vertex qRep,
600 std::map<Vertex, base::State *> &closeRepresentatives,
602{
603 for (auto &closeRepresentative : closeRepresentatives)
604 si_->freeState(closeRepresentative.second);
605 closeRepresentatives.clear();
606
607 // Then, begin searching the space around him
608 for (unsigned int i = 0; i < nearSamplePoints_; ++i)
609 {
610 do
611 {
612 sampler_->sampleNear(workArea, qNew, denseDelta_);
613 } while ((!si_->isValid(workArea) || si_->distance(qNew, workArea) > denseDelta_ ||
614 !si_->checkMotion(qNew, workArea)) &&
615 !ptc);
616
617 // if we were not successful at sampling a desirable state, we are out of time
618 if (ptc)
619 break;
620
621 // Compute who his graph neighbors are
622 Vertex representative = findGraphRepresentative(workArea);
623
624 // Assuming this sample is actually seen by somebody (which he should be in all likelihood)
625 if (representative != boost::graph_traits<Graph>::null_vertex())
626 {
627 // If his representative is different than qNew
628 if (qRep != representative)
629 // And we haven't already tracked this representative
630 if (closeRepresentatives.find(representative) == closeRepresentatives.end())
631 // Track the representative
632 closeRepresentatives[representative] = si_->cloneState(workArea);
633 }
634 else
635 {
636 // This guy can't be seen by anybody, so we should take this opportunity to add him
637 addGuard(si_->cloneState(workArea), COVERAGE);
638
639 // We should also stop our efforts to add a dense path
640 for (auto &closeRepresentative : closeRepresentatives)
641 si_->freeState(closeRepresentative.second);
642 closeRepresentatives.clear();
643 break;
644 }
645 }
646}
647
649{
650 // First of all, we need to compute all candidate r'
651 std::vector<Vertex> VPPs;
652 computeVPP(rep, r, VPPs);
653
654 // Then, for each pair Pv(r,r')
655 foreach (Vertex rp, VPPs)
656 // Try updating the pair info
657 distanceCheck(rep, q, r, s, rp);
658}
659
660void ompl::geometric::SPARStwo::computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs)
661{
662 VPPs.clear();
663 foreach (Vertex cvpp, boost::adjacent_vertices(v, g_))
664 if (cvpp != vp)
665 if (!boost::edge(cvpp, vp, g_).second)
666 VPPs.push_back(cvpp);
667}
668
669void ompl::geometric::SPARStwo::computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs)
670{
671 Xs.clear();
672
673 foreach (Vertex cx, boost::adjacent_vertices(vpp, g_))
674 if (boost::edge(cx, v, g_).second && !boost::edge(cx, vp, g_).second)
675 {
676 InterfaceData &d = getData(v, vpp, cx);
677 if ((vpp < cx && (d.pointA_ != nullptr)) || (cx < vpp && (d.pointB_ != nullptr)))
678 Xs.push_back(cx);
679 }
680 Xs.push_back(vpp);
681}
682
684{
685 if (vp < vpp)
686 return VertexPair(vp, vpp);
687 if (vpp < vp)
688 return VertexPair(vpp, vp);
689 else
690 throw Exception(name_, "Trying to get an index where the pairs are the same point!");
691}
692
697
699 Vertex rp)
700{
701 // Get the info for the current representative-neighbors pair
702 InterfaceData &d = getData(rep, r, rp);
703
704 if (r < rp) // FIRST points represent r (the guy discovered through sampling)
705 {
706 if (d.pointA_ == nullptr) // If the point we're considering replacing (P_v(r,.)) isn't there
707 // Then we know we're doing better, so add it
708 d.setFirst(q, s, si_);
709 else // Otherwise, he is there,
710 {
711 if (d.pointB_ == nullptr) // But if the other guy doesn't exist, we can't compare.
712 {
713 // Should probably keep the one that is further away from rep? Not known what to do in this case.
714 // \todo: is this not part of the algorithm?
715 }
716 else // We know both of these points exist, so we can check some distances
717 if (si_->distance(q, d.pointB_) < si_->distance(d.pointA_, d.pointB_))
718 // Distance with the new point is good, so set it.
719 d.setFirst(q, s, si_);
720 }
721 }
722 else // SECOND points represent r (the guy discovered through sampling)
723 {
724 if (d.pointB_ == nullptr) // If the point we're considering replacing (P_V(.,r)) isn't there...
725 // Then we must be doing better, so add it
726 d.setSecond(q, s, si_);
727 else // Otherwise, he is there
728 {
729 if (d.pointA_ == nullptr) // But if the other guy doesn't exist, we can't compare.
730 {
731 // Should we be doing something cool here?
732 }
733 else if (si_->distance(q, d.pointA_) < si_->distance(d.pointB_, d.pointA_))
734 // Distance with the new point is good, so set it
735 d.setSecond(q, s, si_);
736 }
737 }
738
739 // Lastly, save what we have discovered
740 interfaceDataProperty_[rep][index(r, rp)] = d;
741}
742
744{
746
747 std::vector<Vertex> hold;
748 nn_->nearestR(queryVertex_, sparseDelta_, hold);
749
750 stateProperty_[queryVertex_] = nullptr;
751
752 // For each of the vertices
753 foreach (Vertex v, hold)
754 {
755 foreach (VertexPair r, interfaceDataProperty_[v] | boost::adaptors::map_keys)
756 interfaceDataProperty_[v][r].clear(si_);
757 }
758}
759
761{
762 std::lock_guard<std::mutex> _(graphMutex_);
763
764 Vertex m = boost::add_vertex(g_);
765 stateProperty_[m] = state;
766 colorProperty_[m] = type;
767
768 assert(si_->isValid(state));
769 abandonLists(state);
770
771 disjointSets_.make_set(m);
772 nn_->add(m);
774
775 return m;
776}
777
779{
780 assert(v <= milestoneCount());
781 assert(vp <= milestoneCount());
782
783 const base::Cost weight(costHeuristic(v, vp));
784 const Graph::edge_property_type properties(weight);
785 std::lock_guard<std::mutex> _(graphMutex_);
786 boost::add_edge(v, vp, properties, g_);
787 disjointSets_.union_set(v, vp);
788}
789
790ompl::base::PathPtr ompl::geometric::SPARStwo::constructSolution(const Vertex start, const Vertex goal) const
791{
792 std::lock_guard<std::mutex> _(graphMutex_);
793
794 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
795
796 try
797 {
798 boost::astar_search(
799 g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
800 boost::predecessor_map(prev)
801 .distance_compare([this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
802 .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
803 .distance_inf(opt_->infiniteCost())
804 .distance_zero(opt_->identityCost())
805 .visitor(AStarGoalVisitor<Vertex>(goal)));
806 }
807 catch (AStarFoundGoal &)
808 {
809 }
810
811 if (prev[goal] == goal)
812 throw Exception(name_, "Could not find solution path");
813 else
814 {
815 auto p(std::make_shared<PathGeometric>(si_));
816 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
817 p->append(stateProperty_[pos]);
818 p->append(stateProperty_[start]);
819 p->reverse();
820
821 return p;
822 }
823}
824
825void ompl::geometric::SPARStwo::printDebug(std::ostream &out) const
826{
827 out << "SPARStwo Debug Output: " << std::endl;
828 out << " Settings: " << std::endl;
829 out << " Max Failures: " << getMaxFailures() << std::endl;
830 out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
831 out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
832 out << " Stretch Factor: " << getStretchFactor() << std::endl;
833 out << " Status: " << std::endl;
834 out << " Milestone Count: " << milestoneCount() << std::endl;
835 out << " Iterations: " << getIterationCount() << std::endl;
836 out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
837}
838
840{
841 Planner::getPlannerData(data);
842
843 // Explicitly add start and goal states:
844 for (unsigned long i : startM_)
846
847 for (unsigned long i : goalM_)
849
850 // If there are even edges here
851 if (boost::num_edges(g_) > 0)
852 {
853 // Adding edges and all other vertices simultaneously
854 foreach (const Edge e, boost::edges(g_))
855 {
856 const Vertex v1 = boost::source(e, g_);
857 const Vertex v2 = boost::target(e, g_);
860
861 // Add the reverse edge, since we're constructing an undirected roadmap
864 }
865 }
866 else
867 OMPL_INFORM("%s: There are no edges in the graph!", getName().c_str());
868
869 // Make sure to add edge-less nodes as well
870 foreach (const Vertex n, boost::vertices(g_))
871 if (boost::out_degree(n, g_) == 0)
873}
874
876{
877 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
878}
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition Goal.h:63
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
Definition Goal.h:136
An optimization objective which corresponds to optimizing path length.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
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....
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition Planner.h:403
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
std::string name_
The name of this planner.
Definition Planner.h:419
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
bool setup_
Flag indicating whether setup() has been called.
Definition Planner.h:433
Definition of an abstract state.
Definition State.h:50
bool reachedFailureLimit() const
Returns whether we have reached the iteration failures limit, maxFailures_.
Definition SPARStwo.cpp:209
bool addedSolution_
A flag indicating that a solution has been added during solve()
Definition SPARStwo.h:508
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition SPARStwo.h:496
void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp)
Performs distance checking for the candidate new state, q against the current information.
Definition SPARStwo.cpp:698
std::vector< Vertex > goalM_
Array of goal milestones.
Definition SPARStwo.h:465
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent.
Definition SPARStwo.h:478
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition SPARStwo.h:499
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition SPARStwo.h:244
void resetFailures()
A reset function for resetting the failures count.
Definition SPARStwo.cpp:547
void setStretchFactor(double t)
Sets the stretch factor.
Definition SPARStwo.h:230
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition SPARStwo.h:270
bool checkAddConnectivity(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure connectivity.
Definition SPARStwo.cpp:395
void freeMemory()
Free all the memory allocated by the planner.
Definition SPARStwo.cpp:147
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition SPARStwo.h:532
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition SPARStwo.h:219
base::Cost bestCost_
Best cost found so far by algorithm.
Definition SPARStwo.h:534
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition SPARStwo.h:514
bool checkAddInterface(const base::State *qNew, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the current sample reveals the existence of an interface, and if so,...
Definition SPARStwo.cpp:428
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition SPARStwo.h:276
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition SPARStwo.h:481
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition SPARStwo.h:517
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition SPARStwo.cpp:458
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition SPARStwo.cpp:566
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition SPARStwo.h:236
void computeVPP(Vertex v, Vertex vp, std::vector< Vertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition SPARStwo.cpp:660
bool sameComponent(Vertex m1, Vertex m2)
Check if two milestones (m1 and m2) are part of the same connected component. This is not a const fun...
Definition SPARStwo.cpp:204
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition SPARStwo.h:258
void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)
High-level method which updates pair point information for repV_ with neighbor r.
Definition SPARStwo.cpp:648
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition SPARStwo.h:511
boost::disjoint_sets< boost::property_map< Graph, boost::vertex_rank_t >::type, boost::property_map< Graph, boost::vertex_predecessor_t >::type > disjointSets_
Data structure that maintains the connected components.
Definition SPARStwo.h:503
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition SPARStwo.h:82
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition SPARStwo.h:490
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition SPARStwo.h:95
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition SPARStwo.h:487
std::vector< Vertex > startM_
Array of start milestones.
Definition SPARStwo.h:462
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
Definition SPARStwo.cpp:231
Graph g_
Connectivity graph.
Definition SPARStwo.h:459
std::mutex graphMutex_
Mutex to guard access to the Graph member (g_)
Definition SPARStwo.h:520
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition SPARStwo.h:523
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition SPARStwo.cpp:825
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition SPARStwo.h:456
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition SPARStwo.cpp:683
base::Cost costHeuristic(Vertex u, Vertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition SPARStwo.cpp:875
base::PathPtr constructSolution(Vertex start, Vertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition SPARStwo.cpp:790
SPARStwo(const base::SpaceInformationPtr &si)
Constructor.
Definition SPARStwo.cpp:53
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality)
Definition SPARStwo.h:471
void abandonLists(base::State *st)
When a new guard is added at state st, finds all guards who must abandon their interface information ...
Definition SPARStwo.cpp:743
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition SPARStwo.cpp:365
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches)
Definition SPARStwo.cpp:288
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition SPARStwo.cpp:778
void findCloseRepresentatives(base::State *workArea, const base::State *qNew, Vertex qRep, std::map< Vertex, base::State * > &closeRepresentatives, const base::PlannerTerminationCondition &ptc)
Finds representatives of samples near qNew_ which are not his representative.
Definition SPARStwo.cpp:598
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition SPARStwo.cpp:87
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition SPARStwo.h:216
bool haveSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition SPARStwo.cpp:166
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition SPARStwo.h:484
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition SPARStwo.cpp:693
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition SPARStwo.h:453
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition SPARStwo.h:474
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
Definition SPARStwo.h:493
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition SPARStwo.cpp:298
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st.
Definition SPARStwo.cpp:580
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition SPARStwo.h:264
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition SPARStwo.h:252
void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector< Vertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v', and v".
Definition SPARStwo.cpp:669
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition SPARStwo.cpp:128
bool checkAddCoverage(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure coverage of the space.
Definition SPARStwo.cpp:386
bool reachedTerminationCriterion() const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
Definition SPARStwo.cpp:214
unsigned int milestoneCount() const
Get the number of vertices in the sparse roadmap.
Definition SPARStwo.h:332
~SPARStwo() override
Destructor.
Definition SPARStwo.cpp:82
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition SPARStwo.cpp:135
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition SPARStwo.h:447
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition SPARStwo.h:468
Vertex addGuard(base::State *state, GuardType type)
Construct a guard for a given state (state) and store it in the nearest neighbors data structure.
Definition SPARStwo.cpp:760
void findGraphNeighbors(base::State *st, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near st.
Definition SPARStwo.cpp:552
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition SPARStwo.cpp:839
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ TIMEOUT
The planner failed to find a solution.
@ INFEASIBLE
The planner decided that problem is infeasible.
Interface information storage class, which does bookkeeping for criterion four.
Definition SPARStwo.h:99
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition SPARStwo.h:109
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
Definition SPARStwo.h:105
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Definition SPARStwo.h:101
void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the first interface (i.e. interface with smaller index vertex).
Definition SPARStwo.h:141
void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the second interface (i.e. interface with larger index vertex).
Definition SPARStwo.h:156
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition SPARStwo.h:115