Loading...
Searching...
No Matches
SPARS.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/SPARS.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 "ompl/tools/config/MagicConstants.h"
43#include <thread>
44#include <boost/graph/astar_search.hpp>
45#include <boost/graph/incremental_components.hpp>
46#include <boost/property_map/vector_property_map.hpp>
47#include <boost/foreach.hpp>
48
49#include "GoalVisitor.hpp"
50
51#define foreach BOOST_FOREACH
52#define foreach_reverse BOOST_REVERSE_FOREACH
53
54ompl::geometric::SPARS::SPARS(const base::SpaceInformationPtr &si)
55 : base::Planner(si, "SPARS")
56 , geomPath_(si)
57 , stateProperty_(boost::get(vertex_state_t(), g_))
58 , sparseStateProperty_(boost::get(vertex_state_t(), s_))
59 , sparseColorProperty_(boost::get(vertex_color_t(), s_))
60 , representativesProperty_(boost::get(vertex_representative_t(), g_))
61 , nonInterfaceListsProperty_(boost::get(vertex_list_t(), s_))
62 , interfaceListsProperty_(boost::get(vertex_interface_list_t(), s_))
63 , weightProperty_(boost::get(boost::edge_weight, g_))
64 , sparseDJSets_(boost::get(boost::vertex_rank, s_), boost::get(boost::vertex_predecessor, s_))
65{
69 specs_.multithreaded = true;
70
71 psimp_ = std::make_shared<PathSimplifier>(si_);
72 psimp_->freeStates(false);
73
74 Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:"
75 "3.0");
76 Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction,
77 &SPARS::getSparseDeltaFraction, "0.0:0.01:1.0");
78 Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction,
79 &SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1");
80 Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:"
81 "3000");
82
83 addPlannerProgressProperty("iterations INTEGER", [this] { return getIterationCount(); });
84 addPlannerProgressProperty("best cost REAL", [this] { return getBestCost(); });
85}
86
88{
89 freeMemory();
90}
91
93{
94 Planner::setup();
95 if (!nn_)
96 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(this));
97 nn_->setDistanceFunction([this](const DenseVertex a, const DenseVertex b) { return distanceFunction(a, b); });
98 if (!snn_)
99 snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(this));
100 snn_->setDistanceFunction(
101 [this](const SparseVertex a, const SparseVertex b) { return sparseDistanceFunction(a, b); });
102 if (!connectionStrategy_)
103 connectionStrategy_ =
104 KStarStrategy<DenseVertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
105 double maxExt = si_->getMaximumExtent();
106 sparseDelta_ = sparseDeltaFraction_ * maxExt;
107 denseDelta_ = denseDeltaFraction_ * maxExt;
108
109 // Setup optimization objective
110 //
111 // If no optimization objective was specified, then default to
112 // optimizing path length as computed by the distance() function
113 // in the state space.
114 if (pdef_)
115 {
116 if (pdef_->hasOptimizationObjective())
117 {
118 opt_ = pdef_->getOptimizationObjective();
119 if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()) == nullptr)
120 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
121 "for other optimizaton objectives is not guaranteed.",
122 getName().c_str());
123 }
124 else
125 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
126 }
127 else
128 {
129 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
130 setup_ = false;
131 }
132}
133
134void ompl::geometric::SPARS::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
135{
136 Planner::setProblemDefinition(pdef);
137 clearQuery();
138}
139
141{
142 consecutiveFailures_ = 0;
143}
144
146{
147 startM_.clear();
148 goalM_.clear();
149 pis_.restart();
150
151 // Clear past solutions if there are any
152 if (pdef_)
153 pdef_->clearSolutionPaths();
154}
155
157{
158 Planner::clear();
159 sampler_.reset();
160 freeMemory();
161 if (nn_)
162 nn_->clear();
163 if (snn_)
164 snn_->clear();
165 clearQuery();
166 resetFailures();
167 iterations_ = 0;
168 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
169}
170
172{
173 foreach (DenseVertex v, boost::vertices(g_))
174 if (stateProperty_[v] != nullptr)
175 {
176 si_->freeState(stateProperty_[v]);
177 stateProperty_[v] = nullptr;
178 }
179 foreach (SparseVertex n, boost::vertices(s_))
180 if (sparseStateProperty_[n] != nullptr)
181 {
182 si_->freeState(sparseStateProperty_[n]);
183 sparseStateProperty_[n] = nullptr;
184 }
185 s_.clear();
186 g_.clear();
187}
188
191{
192 DenseVertex result = boost::graph_traits<DenseGraph>::null_vertex();
193
194 // search for a valid state
195 bool found = false;
196 while (!found && !ptc)
197 {
198 unsigned int attempts = 0;
199 do
200 {
201 found = sampler_->sample(workState);
202 attempts++;
204 }
205
206 if (found)
207 result = addMilestone(si_->cloneState(workState));
208 return result;
209}
210
212{
213 auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
214 while (!ptc && !addedSolution_)
215 {
216 // Check for any new goal states
217 if (goal->maxSampleCount() > goalM_.size())
218 {
219 const base::State *st = pis_.nextGoal();
220 if (st != nullptr)
221 {
222 addMilestone(si_->cloneState(st));
223 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
224 }
225 }
226
227 // Check for a solution
228 addedSolution_ = haveSolution(startM_, goalM_, solution);
229 // Sleep for 1ms
230 if (!addedSolution_)
231 std::this_thread::sleep_for(std::chrono::milliseconds(1));
232 }
233}
234
235bool ompl::geometric::SPARS::haveSolution(const std::vector<DenseVertex> &starts, const std::vector<DenseVertex> &goals,
236 base::PathPtr &solution)
237{
238 base::Goal *g = pdef_->getGoal().get();
239 base::Cost sol_cost(opt_->infiniteCost());
240 foreach (DenseVertex start, starts)
241 {
242 foreach (DenseVertex goal, goals)
243 {
244 // we lock because the connected components algorithm is incremental and may change disjointSets_
245 graphMutex_.lock();
246 bool same_component = sameComponent(start, goal);
247 graphMutex_.unlock();
248
249 if (same_component && g->isStartGoalPairValid(sparseStateProperty_[goal], sparseStateProperty_[start]))
250 {
251 base::PathPtr p = constructSolution(start, goal);
252 if (p)
253 {
254 base::Cost pathCost = p->cost(opt_);
255 if (opt_->isCostBetterThan(pathCost, bestCost_))
256 bestCost_ = pathCost;
257 // Check if optimization objective is satisfied
258 if (opt_->isSatisfied(pathCost))
259 {
260 solution = p;
261 return true;
262 }
263 if (opt_->isCostBetterThan(pathCost, sol_cost))
264 {
265 solution = p;
266 sol_cost = pathCost;
267 }
268 }
269 }
270 }
271 }
272
273 return false;
274}
275
277{
278 return consecutiveFailures_ >= maxFailures_ || addedSolution_;
279}
280
282{
283 return consecutiveFailures_ >= maxFailures_;
284}
285
287{
288 std::lock_guard<std::mutex> _(graphMutex_);
289 if (boost::num_vertices(g_) < 1)
290 {
291 sparseQueryVertex_ = boost::add_vertex(s_);
292 queryVertex_ = boost::add_vertex(g_);
293 sparseStateProperty_[sparseQueryVertex_] = nullptr;
294 stateProperty_[queryVertex_] = nullptr;
295 }
296}
297
299{
300 return boost::same_component(m1, m2, sparseDJSets_);
301}
302
304{
305 checkValidity();
306 checkQueryStateInitialization();
307
308 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
309
310 if (goal == nullptr)
311 {
312 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
314 }
315
316 // Add the valid start states as milestones
317 while (const base::State *st = pis_.nextStart())
318 {
319 addMilestone(si_->cloneState(st));
320 startM_.push_back(addGuard(si_->cloneState(st), START));
321 }
322 if (startM_.empty())
323 {
324 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
326 }
327
328 if (goalM_.empty() && !goal->couldSample())
329 {
330 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
332 }
333
334 // Add the valid goal states as milestones
335 while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
336 {
337 addMilestone(si_->cloneState(st));
338 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
339 }
340 if (goalM_.empty())
341 {
342 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
344 }
345
346 unsigned int nrStartStatesDense = boost::num_vertices(g_) - 1; // don't count query vertex
347 unsigned int nrStartStatesSparse = boost::num_vertices(s_) - 1; // don't count query vertex
348 OMPL_INFORM("%s: Starting planning with %u dense states, %u sparse states", getName().c_str(), nrStartStatesDense,
349 nrStartStatesSparse);
350
351 // Reset addedSolution_ member
352 addedSolution_ = false;
353 resetFailures();
354 base::PathPtr sol;
355 base::PlannerTerminationCondition ptcOrFail([this, &ptc] { return ptc || reachedFailureLimit(); });
356 std::thread slnThread([this, &ptcOrFail, &sol] { checkForSolution(ptcOrFail, sol); });
357
358 // Construct planner termination condition which also takes maxFailures_ and addedSolution_ into account
359 base::PlannerTerminationCondition ptcOrStop([this, &ptc] { return ptc || reachedTerminationCriterion(); });
360 constructRoadmap(ptcOrStop);
361
362 // Ensure slnThread is ceased before exiting solve
363 slnThread.join();
364
365 if (sol)
366 pdef_->addSolutionPath(sol, false, -1.0, getName());
367
368 OMPL_INFORM("%s: Created %u dense states, %u sparse states", getName().c_str(),
369 (unsigned int)(boost::num_vertices(g_) - nrStartStatesDense),
370 (unsigned int)(boost::num_vertices(s_) - nrStartStatesSparse));
371
372 // Return true if any solution was found.
373 if (sol)
374 {
376 }
377 else
378 {
379 return reachedFailureLimit() ? base::PlannerStatus::INFEASIBLE : base::PlannerStatus::TIMEOUT;
380 }
381}
382
384{
385 if (stopOnMaxFail)
386 {
387 resetFailures();
388 base::PlannerTerminationCondition ptcOrFail([this, &ptc] { return ptc || reachedFailureLimit(); });
389 constructRoadmap(ptcOrFail);
390 }
391 else
392 constructRoadmap(ptc);
393}
394
396{
397 checkQueryStateInitialization();
398
399 if (!isSetup())
400 setup();
401 if (!sampler_)
402 sampler_ = si_->allocValidStateSampler();
403
404 base::State *workState = si_->allocState();
405
406 /* The whole neighborhood set which has been most recently computed */
407 std::vector<SparseVertex> graphNeighborhood;
408
409 /* The visible neighborhood set which has been most recently computed */
410 std::vector<SparseVertex> visibleNeighborhood;
411
412 /* Storage for the interface neighborhood, populated by getInterfaceNeighborhood() */
413 std::vector<DenseVertex> interfaceNeighborhood;
414
415 bestCost_ = opt_->infiniteCost();
416 while (!ptc)
417 {
418 iterations_++;
419
420 // Generate a single sample, and attempt to connect it to nearest neighbors.
421 DenseVertex q = addSample(workState, ptc);
422 if (q == boost::graph_traits<DenseGraph>::null_vertex())
423 continue;
424
425 // Now that we've added to D, try adding to S
426 // Start by figuring out who our neighbors are
427 getSparseNeighbors(workState, graphNeighborhood);
428 filterVisibleNeighbors(workState, graphNeighborhood, visibleNeighborhood);
429 // Check for addition for Coverage
430 if (!checkAddCoverage(workState, graphNeighborhood))
431 // If not for Coverage, then Connectivity
432 if (!checkAddConnectivity(workState, graphNeighborhood))
433 // Check for the existence of an interface
434 if (!checkAddInterface(graphNeighborhood, visibleNeighborhood, q))
435 {
436 // Then check to see if it's on an interface
437 getInterfaceNeighborhood(q, interfaceNeighborhood);
438 if (!interfaceNeighborhood.empty())
439 {
440 // Check for addition for spanner prop
441 if (!checkAddPath(q, interfaceNeighborhood))
442 // All of the tests have failed. Report failure for the sample
443 ++consecutiveFailures_;
444 }
445 else
446 // There's no interface here, so drop it
447 ++consecutiveFailures_;
448 }
449 }
450
451 si_->freeState(workState);
452}
453
455{
456 std::lock_guard<std::mutex> _(graphMutex_);
457
458 DenseVertex m = boost::add_vertex(g_);
459 stateProperty_[m] = state;
460
461 // Which milestones will we attempt to connect to?
462 const std::vector<DenseVertex> &neighbors = connectionStrategy_(m);
463
464 foreach (DenseVertex n, neighbors)
465 if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
466 {
467 const double weight = distanceFunction(m, n);
468 const DenseGraph::edge_property_type properties(weight);
469
470 boost::add_edge(m, n, properties, g_);
471 }
472
473 nn_->add(m);
474
475 // Need to update representative information here...
476 calculateRepresentative(m);
477
478 std::vector<DenseVertex> interfaceNeighborhood;
479 std::set<SparseVertex> interfaceRepresentatives;
480
481 getInterfaceNeighborRepresentatives(m, interfaceRepresentatives);
482 getInterfaceNeighborhood(m, interfaceNeighborhood);
483 addToRepresentatives(m, representativesProperty_[m], interfaceRepresentatives);
484 foreach (DenseVertex qp, interfaceNeighborhood)
485 {
486 removeFromRepresentatives(qp, representativesProperty_[qp]);
487 getInterfaceNeighborRepresentatives(qp, interfaceRepresentatives);
488 addToRepresentatives(qp, representativesProperty_[qp], interfaceRepresentatives);
489 }
490
491 return m;
492}
493
495{
496 std::lock_guard<std::mutex> _(graphMutex_);
497
498 SparseVertex v = boost::add_vertex(s_);
499 sparseStateProperty_[v] = state;
500 sparseColorProperty_[v] = type;
501
502 sparseDJSets_.make_set(v);
503
504 snn_->add(v);
505 updateRepresentatives(v);
506
507 resetFailures();
508 return v;
509}
510
512{
513 const base::Cost weight(costHeuristic(v, vp));
514 const SpannerGraph::edge_property_type properties(weight);
515 std::lock_guard<std::mutex> _(graphMutex_);
516 boost::add_edge(v, vp, properties, s_);
517 sparseDJSets_.union_set(v, vp);
518}
519
521{
522 const double weight = distanceFunction(v, vp);
523 const DenseGraph::edge_property_type properties(weight);
524 std::lock_guard<std::mutex> _(graphMutex_);
525 boost::add_edge(v, vp, properties, g_);
526}
527
528bool ompl::geometric::SPARS::checkAddCoverage(const base::State *lastState, const std::vector<SparseVertex> &neigh)
529{
530 // For each of these neighbors,
531 foreach (SparseVertex n, neigh)
532 // If path between is free
533 if (si_->checkMotion(lastState, sparseStateProperty_[n]))
534 // Abort out and return false
535 return false;
536 // No free paths means we add for coverage
537 addGuard(si_->cloneState(lastState), COVERAGE);
538 return true;
539}
540
541bool ompl::geometric::SPARS::checkAddConnectivity(const base::State *lastState, const std::vector<SparseVertex> &neigh)
542{
543 std::vector<SparseVertex> links;
544 // For each neighbor
545 for (std::size_t i = 0; i < neigh.size(); ++i)
546 // For each other neighbor
547 for (std::size_t j = i + 1; j < neigh.size(); ++j)
548 // If they are in different components
549 if (!sameComponent(neigh[i], neigh[j]))
550 // If the paths between are collision free
551 if (si_->checkMotion(lastState, sparseStateProperty_[neigh[i]]) &&
552 si_->checkMotion(lastState, sparseStateProperty_[neigh[j]]))
553 {
554 links.push_back(neigh[i]);
555 links.push_back(neigh[j]);
556 }
557
558 if (!links.empty())
559 {
560 // Add the node
561 SparseVertex g = addGuard(si_->cloneState(lastState), CONNECTIVITY);
562
563 for (unsigned long link : links)
564 // If there's no edge
565 if (!boost::edge(g, link, s_).second)
566 // And the components haven't been united by previous links
567 if (!sameComponent(link, g))
568 connectSparsePoints(g, link);
569 return true;
570 }
571 return false;
572}
573
574bool ompl::geometric::SPARS::checkAddInterface(const std::vector<SparseVertex> &graphNeighborhood,
575 const std::vector<SparseVertex> &visibleNeighborhood, DenseVertex q)
576{
577 // If we have more than 1 neighbor
578 if (visibleNeighborhood.size() > 1)
579 // If our closest neighbors are also visible
580 if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
581 // If our two closest neighbors don't share an edge
582 if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], s_).second)
583 {
584 // If they can be directly connected
585 if (si_->checkMotion(sparseStateProperty_[visibleNeighborhood[0]],
586 sparseStateProperty_[visibleNeighborhood[1]]))
587 {
588 // Connect them
589 connectSparsePoints(visibleNeighborhood[0], visibleNeighborhood[1]);
590 // And report that we added to the roadmap
591 resetFailures();
592 // Report success
593 return true;
594 }
595
596 // Add the new node to the graph, to bridge the interface
597 SparseVertex v = addGuard(si_->cloneState(stateProperty_[q]), INTERFACE);
598 connectSparsePoints(v, visibleNeighborhood[0]);
599 connectSparsePoints(v, visibleNeighborhood[1]);
600 // Report success
601 return true;
602 }
603 return false;
604}
605
606bool ompl::geometric::SPARS::checkAddPath(DenseVertex q, const std::vector<DenseVertex> &neigh)
607{
608 bool result = false;
609
610 // Get q's representative => v
611 SparseVertex v = representativesProperty_[q];
612
613 // Extract the representatives of neigh => n_rep
614 std::set<SparseVertex> n_rep;
615 foreach (DenseVertex qp, neigh)
616 n_rep.insert(representativesProperty_[qp]);
617
618 std::vector<SparseVertex> Xs;
619 // for each v' in n_rep
620 for (auto it = n_rep.begin(); it != n_rep.end() && !result; ++it)
621 {
622 SparseVertex vp = *it;
623 // Identify appropriate v" candidates => vpps
624 std::vector<SparseVertex> VPPs;
625 computeVPP(v, vp, VPPs);
626
627 foreach (SparseVertex vpp, VPPs)
628 {
629 double s_max = 0;
630 // Find the X nodes to test
631 computeX(v, vp, vpp, Xs);
632
633 // For each x in xs
634 foreach (SparseVertex x, Xs)
635 {
636 // Compute/Retain MAXimum distance path thorugh S
637 double dist = (si_->distance(sparseStateProperty_[x], sparseStateProperty_[v]) +
638 si_->distance(sparseStateProperty_[v], sparseStateProperty_[vp])) /
639 2.0;
640 if (dist > s_max)
641 s_max = dist;
642 }
643
644 DensePath bestDPath;
645 DenseVertex best_qpp = boost::graph_traits<DenseGraph>::null_vertex();
646 double d_min = std::numeric_limits<double>::infinity(); // Insanely big number
647 // For each vpp in vpps
648 for (std::size_t j = 0; j < VPPs.size() && !result; ++j)
649 {
650 SparseVertex vpp = VPPs[j];
651 // For each q", which are stored interface nodes on v for i(vpp,v)
652 foreach (DenseVertex qpp, interfaceListsProperty_[v][vpp])
653 {
654 // check that representatives are consistent
655 assert(representativesProperty_[qpp] == v);
656
657 // If they happen to be the one and same node
658 if (q == qpp)
659 {
660 bestDPath.push_front(stateProperty_[q]);
661 best_qpp = qpp;
662 d_min = 0;
663 }
664 else
665 {
666 // Compute/Retain MINimum distance path on D through q, q"
667 DensePath dPath;
668 computeDensePath(q, qpp, dPath);
669 if (!dPath.empty())
670 {
671 // compute path length
672 double length = 0.0;
673 DensePath::const_iterator jt = dPath.begin();
674 for (auto it = jt + 1; it != dPath.end(); ++it)
675 {
676 length += si_->distance(*jt, *it);
677 jt = it;
678 }
679
680 if (length < d_min)
681 {
682 d_min = length;
683 bestDPath.swap(dPath);
684 best_qpp = qpp;
685 }
686 }
687 }
688 }
689
690 // If the spanner property is violated for these paths
691 if (s_max > stretchFactor_ * d_min)
692 {
693 // Need to augment this path with the appropriate neighbor information
694 DenseVertex na = getInterfaceNeighbor(q, vp);
695 DenseVertex nb = getInterfaceNeighbor(best_qpp, vpp);
696
697 bestDPath.push_front(stateProperty_[na]);
698 bestDPath.push_back(stateProperty_[nb]);
699
700 // check consistency of representatives
701 assert(representativesProperty_[na] == vp && representativesProperty_[nb] == vpp);
702
703 // Add the dense path to the spanner
704 addPathToSpanner(bestDPath, vpp, vp);
705
706 // Report success
707 result = true;
708 }
709 }
710 }
711 }
712 return result;
713}
714
716{
717 double degree = 0.0;
718 foreach (DenseVertex v, boost::vertices(s_))
719 degree += (double)boost::out_degree(v, s_);
720 degree /= (double)boost::num_vertices(s_);
721 return degree;
722}
723
724void ompl::geometric::SPARS::printDebug(std::ostream &out) const
725{
726 out << "SPARS Debug Output: " << std::endl;
727 out << " Settings: " << std::endl;
728 out << " Max Failures: " << getMaxFailures() << std::endl;
729 out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
730 out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
731 out << " Stretch Factor: " << getStretchFactor() << std::endl;
732 out << " Status: " << std::endl;
733 out << " Milestone Count: " << milestoneCount() << std::endl;
734 out << " Guard Count: " << guardCount() << std::endl;
735 out << " Iterations: " << getIterationCount() << std::endl;
736 out << " Average Valence: " << averageValence() << std::endl;
737 out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
738}
739
740void ompl::geometric::SPARS::getSparseNeighbors(base::State *inState, std::vector<SparseVertex> &graphNeighborhood)
741{
742 sparseStateProperty_[sparseQueryVertex_] = inState;
743
744 graphNeighborhood.clear();
745 snn_->nearestR(sparseQueryVertex_, sparseDelta_, graphNeighborhood);
746
747 sparseStateProperty_[sparseQueryVertex_] = nullptr;
748}
749
751 const std::vector<SparseVertex> &graphNeighborhood,
752 std::vector<SparseVertex> &visibleNeighborhood) const
753{
754 visibleNeighborhood.clear();
755 // Now that we got the neighbors from the NN, we must remove any we can't see
756 for (unsigned long i : graphNeighborhood)
757 if (si_->checkMotion(inState, sparseStateProperty_[i]))
758 visibleNeighborhood.push_back(i);
759}
760
762{
763 foreach (DenseVertex vp, boost::adjacent_vertices(q, g_))
764 if (representativesProperty_[vp] == rep)
765 if (distanceFunction(q, vp) <= denseDelta_)
766 return vp;
767 throw Exception(name_, "Vertex has no interface neighbor with given representative");
768}
769
771{
772 // First, check to see that the path has length
773 if (dense_path.size() <= 1)
774 {
775 // The path is 0 length, so simply link the representatives
776 connectSparsePoints(vp, vpp);
777 resetFailures();
778 }
779 else
780 {
781 // We will need to construct a PathGeometric to do this.
782 geomPath_.getStates().resize(dense_path.size());
783 std::copy(dense_path.begin(), dense_path.end(), geomPath_.getStates().begin());
784
785 // Attempt to simplify the path
786 psimp_->reduceVertices(geomPath_, geomPath_.getStateCount() * 2);
787
788 // we are sure there are at least 2 points left on geomPath_
789
790 std::vector<SparseVertex> added_nodes;
791 added_nodes.reserve(geomPath_.getStateCount());
792 for (std::size_t i = 0; i < geomPath_.getStateCount(); ++i)
793 {
794 // Add each guard
795 SparseVertex ng = addGuard(si_->cloneState(geomPath_.getState(i)), QUALITY);
796 added_nodes.push_back(ng);
797 }
798 // Link them up
799 for (std::size_t i = 1; i < added_nodes.size(); ++i)
800 {
801 connectSparsePoints(added_nodes[i - 1], added_nodes[i]);
802 }
803 // Don't forget to link them to their representatives
804 connectSparsePoints(added_nodes[0], vp);
805 connectSparsePoints(added_nodes[added_nodes.size() - 1], vpp);
806 }
807 geomPath_.getStates().clear();
808 return true;
809}
810
812{
813 // Get all of the dense samples which may be affected by adding this node
814 std::vector<DenseVertex> dense_points;
815
816 stateProperty_[queryVertex_] = sparseStateProperty_[v];
817
818 nn_->nearestR(queryVertex_, sparseDelta_ + denseDelta_, dense_points);
819
820 stateProperty_[queryVertex_] = nullptr;
821
822 // For each of those points
823 for (unsigned long dense_point : dense_points)
824 {
825 // Remove that point from the old representative's list(s)
826 removeFromRepresentatives(dense_point, representativesProperty_[dense_point]);
827 // Update that point's representative
828 calculateRepresentative(dense_point);
829 }
830
831 std::set<SparseVertex> interfaceRepresentatives;
832 // For each of the points
833 for (unsigned long dense_point : dense_points)
834 {
835 // Get it's representative
836 SparseVertex rep = representativesProperty_[dense_point];
837 // Extract the representatives of any interface-sharing neighbors
838 getInterfaceNeighborRepresentatives(dense_point, interfaceRepresentatives);
839 // For sanity's sake, make sure we clear ourselves out of what this new rep might think of us
840 removeFromRepresentatives(dense_point, rep);
841 // Add this vertex to it's representative's list for the other representatives
842 addToRepresentatives(dense_point, rep, interfaceRepresentatives);
843 }
844}
845
847{
848 // Get the nearest neighbors within sparseDelta_
849 std::vector<SparseVertex> graphNeighborhood;
850 getSparseNeighbors(stateProperty_[q], graphNeighborhood);
851
852 // For each neighbor
853 for (unsigned long i : graphNeighborhood)
854 if (si_->checkMotion(stateProperty_[q], sparseStateProperty_[i]))
855 {
856 // update the representative
857 representativesProperty_[q] = i;
858 // abort
859 break;
860 }
861}
862
863void ompl::geometric::SPARS::addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set<SparseVertex> &oreps)
864{
865 // If this node supports no interfaces
866 if (oreps.empty())
867 {
868 // Add it to the pool of non-interface nodes
869 bool new_insert = nonInterfaceListsProperty_[rep].insert(q).second;
870
871 // we expect this was not previously tracked
872 if (!new_insert)
873 assert(false);
874 }
875 else
876 {
877 // otherwise, for every neighbor representative
878 foreach (SparseVertex v, oreps)
879 {
880 assert(rep == representativesProperty_[q]);
881 bool new_insert = interfaceListsProperty_[rep][v].insert(q).second;
882 if (!new_insert)
883 assert(false);
884 }
885 }
886}
887
889{
890 // Remove the node from the non-interface points (if there)
891 nonInterfaceListsProperty_[rep].erase(q);
892
893 // From each of the interfaces
894 foreach (SparseVertex vpp, interfaceListsProperty_[rep] | boost::adaptors::map_keys)
895 {
896 // Remove this node from that list
897 interfaceListsProperty_[rep][vpp].erase(q);
898 }
899}
900
901void ompl::geometric::SPARS::computeVPP(SparseVertex v, SparseVertex vp, std::vector<SparseVertex> &VPPs)
902{
903 foreach (SparseVertex cvpp, boost::adjacent_vertices(v, s_))
904 if (cvpp != vp)
905 if (!boost::edge(cvpp, vp, s_).second)
906 VPPs.push_back(cvpp);
907}
908
909void ompl::geometric::SPARS::computeX(SparseVertex v, SparseVertex vp, SparseVertex vpp, std::vector<SparseVertex> &Xs)
910{
911 Xs.clear();
912 foreach (SparseVertex cx, boost::adjacent_vertices(vpp, s_))
913 if (boost::edge(cx, v, s_).second && !boost::edge(cx, vp, s_).second)
914 if (!interfaceListsProperty_[vpp][cx].empty())
915 Xs.push_back(cx);
916 Xs.push_back(vpp);
917}
918
920 std::set<SparseVertex> &interfaceRepresentatives)
921{
922 interfaceRepresentatives.clear();
923
924 // Get our representative
925 SparseVertex rep = representativesProperty_[q];
926 // For each neighbor we are connected to
927 foreach (DenseVertex n, boost::adjacent_vertices(q, g_))
928 {
929 // Get his representative
930 SparseVertex orep = representativesProperty_[n];
931 // If that representative is not our own
932 if (orep != rep)
933 // If he is within denseDelta_
934 if (distanceFunction(q, n) < denseDelta_)
935 // Include his rep in the set
936 interfaceRepresentatives.insert(orep);
937 }
938}
939
940void ompl::geometric::SPARS::getInterfaceNeighborhood(DenseVertex q, std::vector<DenseVertex> &interfaceNeighborhood)
941{
942 interfaceNeighborhood.clear();
943
944 // Get our representative
945 SparseVertex rep = representativesProperty_[q];
946
947 // For each neighbor we are connected to
948 foreach (DenseVertex n, boost::adjacent_vertices(q, g_))
949 // If neighbor representative is not our own
950 if (representativesProperty_[n] != rep)
951 // If he is within denseDelta_
952 if (distanceFunction(q, n) < denseDelta_)
953 // Append him to the list
954 interfaceNeighborhood.push_back(n);
955}
956
958{
959 std::lock_guard<std::mutex> _(graphMutex_);
960
961 boost::vector_property_map<SparseVertex> prev(boost::num_vertices(s_));
962
963 try
964 {
965 // Consider using a persistent distance_map if it's slow
966 boost::astar_search(
967 s_, start, [this, goal](SparseVertex v) { return costHeuristic(v, goal); },
968 boost::predecessor_map(prev)
969 .distance_compare([this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
970 .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
971 .distance_inf(opt_->infiniteCost())
972 .distance_zero(opt_->identityCost())
973 .visitor(AStarGoalVisitor<SparseVertex>(goal)));
974 }
975 catch (AStarFoundGoal &)
976 {
977 }
978
979 if (prev[goal] == goal)
980 throw Exception(name_, "Could not find solution path");
981 else
982 {
983 auto p(std::make_shared<PathGeometric>(si_));
984
985 for (SparseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
986 p->append(sparseStateProperty_[pos]);
987 p->append(sparseStateProperty_[start]);
988 p->reverse();
989
990 return p;
991 }
992}
993
995{
996 path.clear();
997
998 boost::vector_property_map<DenseVertex> prev(boost::num_vertices(g_));
999
1000 try
1001 {
1002 boost::astar_search(g_, start, [this, goal](const DenseVertex a) { return distanceFunction(a, goal); },
1003 boost::predecessor_map(prev).visitor(AStarGoalVisitor<DenseVertex>(goal)));
1004 }
1005 catch (AStarFoundGoal &)
1006 {
1007 }
1008
1009 if (prev[goal] == goal)
1010 OMPL_WARN("%s: No dense path was found?", getName().c_str());
1011 else
1012 {
1013 for (DenseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
1014 path.push_front(stateProperty_[pos]);
1015 path.push_front(stateProperty_[start]);
1016 }
1017}
1018
1020{
1021 Planner::getPlannerData(data);
1022
1023 // Explicitly add start and goal states:
1024 for (unsigned long i : startM_)
1025 data.addStartVertex(base::PlannerDataVertex(sparseStateProperty_[i], (int)START));
1026
1027 for (unsigned long i : goalM_)
1028 data.addGoalVertex(base::PlannerDataVertex(sparseStateProperty_[i], (int)GOAL));
1029
1030 // Adding edges and all other vertices simultaneously
1031 foreach (const SparseEdge e, boost::edges(s_))
1032 {
1033 const SparseVertex v1 = boost::source(e, s_);
1034 const SparseVertex v2 = boost::target(e, s_);
1035 data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v1]),
1036 base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v2]));
1037
1038 // Add the reverse edge, since we're constructing an undirected roadmap
1039 data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v1]),
1040 base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v2]));
1041 }
1042
1043 // Make sure to add edge-less nodes as well
1044 foreach (const SparseVertex n, boost::vertices(s_))
1045 if (boost::out_degree(n, s_) == 0)
1046 data.addVertex(base::PlannerDataVertex(sparseStateProperty_[n], (int)sparseColorProperty_[n]));
1047}
1048
1050{
1051 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
1052}
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:403
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
Definition of an abstract state.
Definition State.h:50
Make the minimal number of connections required to ensure asymptotic optimality.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition SPARS.cpp:92
bool checkAddPath(DenseVertex q, const std::vector< DenseVertex > &neigh)
Checks for adding an entire dense path to the Sparse Roadmap.
Definition SPARS.cpp:606
bool reachedFailureLimit() const
Returns true if we have reached the iteration failures limit, maxFailures_
Definition SPARS.cpp:281
void getInterfaceNeighborRepresentatives(DenseVertex q, std::set< SparseVertex > &interfaceRepresentatives)
Gets the representatives of all interfaces that q supports.
Definition SPARS.cpp:919
void getSparseNeighbors(base::State *inState, std::vector< SparseVertex > &graphNeighborhood)
Get all nodes in the sparse graph which are within sparseDelta_ of the given state.
Definition SPARS.cpp:740
void computeX(DenseVertex v, DenseVertex vp, DenseVertex vpp, std::vector< SparseVertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v', and v".
Definition SPARS.cpp:909
boost::graph_traits< DenseGraph >::vertex_descriptor DenseVertex
A vertex in DenseGraph.
Definition SPARS.h:184
~SPARS() override
Destructor.
Definition SPARS.cpp:87
DenseVertex addMilestone(base::State *state)
Construct a milestone for a given state (state) and store it in the nearest neighbors data structure.
Definition SPARS.cpp:454
void setStretchFactor(double t)
Set the roadmap spanner stretch factor. This value represents a multiplicative upper bound on path qu...
Definition SPARS.h:292
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches)
Definition SPARS.cpp:286
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 SPARS.cpp:1019
void connectDensePoints(DenseVertex v, DenseVertex vp)
Connects points in the dense graph.
Definition SPARS.cpp:520
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition SPARS.cpp:156
base::Cost costHeuristic(SparseVertex u, SparseVertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition SPARS.cpp:1049
bool reachedTerminationCriterion() const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
Definition SPARS.cpp:276
base::PathPtr constructSolution(SparseVertex start, SparseVertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition SPARS.cpp:957
void setDenseDeltaFraction(double d)
Set the delta fraction for interface detection. If two nodes in the dense graph are more than a delta...
Definition SPARS.h:269
void updateRepresentatives(SparseVertex v)
Automatically updates the representatives of all dense samplse within sparseDelta_ of v.
Definition SPARS.cpp:811
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta fraction.
Definition SPARS.h:304
bool checkAddInterface(const std::vector< DenseVertex > &graphNeighborhood, const std::vector< DenseVertex > &visibleNeighborhood, DenseVertex q)
Checks the latest dense sample for bridging an edge-less interface.
Definition SPARS.cpp:574
SparseVertex addGuard(base::State *state, GuardType type)
Construct a node with the given state (state) for the spanner and store it in the nn structure.
Definition SPARS.cpp:494
std::deque< base::State * > DensePath
Internal representation of a dense path.
Definition SPARS.h:123
void computeDensePath(DenseVertex start, DenseVertex goal, DensePath &path) const
Constructs the dense path between the start and goal vertices (if connected)
Definition SPARS.cpp:994
boost::graph_traits< SpannerGraph >::edge_descriptor SparseEdge
An edge in the sparse roadmap that is constructed.
Definition SPARS.h:154
boost::graph_traits< SpannerGraph >::vertex_descriptor SparseVertex
A vertex in the sparse roadmap that is constructed.
Definition SPARS.h:151
double averageValence() const
Returns the average valence of the spanner graph.
Definition SPARS.cpp:715
bool checkAddConnectivity(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for connectivity, and adds appropriately.
Definition SPARS.cpp:541
DenseVertex getInterfaceNeighbor(DenseVertex q, SparseVertex rep)
Get the first neighbor of q who has representative rep and is within denseDelta_.
Definition SPARS.cpp:761
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition SPARS.cpp:145
unsigned getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition SPARS.h:298
void calculateRepresentative(DenseVertex q)
Calculates the representative for a dense sample.
Definition SPARS.cpp:846
void setSparseDeltaFraction(double d)
Set the delta fraction for connection distance on the sparse spanner. This value represents the visib...
Definition SPARS.h:280
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition SPARS.cpp:724
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
Definition SPARS.cpp:395
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to S.
Definition SPARS.h:531
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 SPARS.cpp:303
bool haveSolution(const std::vector< DenseVertex > &starts, const std::vector< DenseVertex > &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 SPARS.cpp:235
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition SPARS.h:82
void connectSparsePoints(SparseVertex v, SparseVertex vp)
Convenience function for creating an edge in the Spanner Roadmap.
Definition SPARS.cpp:511
void freeMemory()
Free all the memory allocated by the planner.
Definition SPARS.cpp:171
bool addPathToSpanner(const DensePath &dense_path, SparseVertex vp, SparseVertex vpp)
Method for actually adding a dense path to the Roadmap Spanner, S.
Definition SPARS.cpp:770
void addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set< SparseVertex > &oreps)
Adds a dense sample to the appropriate lists of its representative.
Definition SPARS.cpp:863
bool checkAddCoverage(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for the coverage property, and adds appropriately.
Definition SPARS.cpp:528
void resetFailures()
A reset function for resetting the failures count.
Definition SPARS.cpp:140
void setMaxFailures(unsigned int m)
Set the maximum consecutive failures to augment the spanner before termination. In general,...
Definition SPARS.h:261
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition SPARS.h:316
void computeVPP(DenseVertex v, DenseVertex vp, std::vector< SparseVertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition SPARS.cpp:901
DenseVertex addSample(base::State *workState, const base::PlannerTerminationCondition &ptc)
Attempt to add a single sample to the roadmap.
Definition SPARS.cpp:189
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta fraction.
Definition SPARS.h:310
void getInterfaceNeighborhood(DenseVertex q, std::vector< DenseVertex > &interfaceNeighborhood)
Gets the neighbors of q who help it support an interface.
Definition SPARS.cpp:940
void filterVisibleNeighbors(base::State *inState, const std::vector< SparseVertex > &graphNeighborhood, std::vector< SparseVertex > &visibleNeighborhood) const
Get the visible neighbors.
Definition SPARS.cpp:750
void removeFromRepresentatives(DenseVertex q, SparseVertex rep)
Removes the node from its representative's lists.
Definition SPARS.cpp:888
bool sameComponent(SparseVertex m1, SparseVertex m2)
Check that two vertices are in the same connected component.
Definition SPARS.cpp:298
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition SPARS.cpp:211
SPARS(const base::SpaceInformationPtr &si)
Constructor.
Definition SPARS.cpp:54
#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
static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK
Maximum number of sampling attempts to find a valid state, without checking whether the allowed time ...
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition Planner.h:192
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition Planner.h:199
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition Planner.h:189
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition Planner.h:195
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.