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