Loading...
Searching...
No Matches
PRM.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
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 Rice 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: Ioan Sucan, James D. Marble, Ryan Luna, Henning Kayser */
36
37#include "ompl/geometric/planners/prm/PRM.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/datastructures/PDF.h"
42#include "ompl/tools/config/SelfConfig.h"
43#include "ompl/tools/config/MagicConstants.h"
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#include <thread>
49#include <typeinfo>
50
51#include "GoalVisitor.hpp"
52
53#define foreach BOOST_FOREACH
54
55namespace ompl
56{
57 namespace magic
58 {
61 static const unsigned int MAX_RANDOM_BOUNCE_STEPS = 5;
62
64 static const double ROADMAP_BUILD_TIME = 0.2;
65
68 static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
69 } // namespace magic
70} // namespace ompl
71
72ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy)
73 : base::Planner(si, "PRM")
74 , starStrategy_(starStrategy)
75 , stateProperty_(boost::get(vertex_state_t(), g_))
76 , totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_))
77 , successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_))
78 , weightProperty_(boost::get(boost::edge_weight, g_))
79 , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
80{
84 specs_.multithreaded = true;
85
86 if (!starStrategy_)
87 Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors,
88 &PRM::getMaxNearestNeighbors, std::string("8:1000"));
89
90 addPlannerProgressProperty("iterations INTEGER", [this] { return getIterationCount(); });
91 addPlannerProgressProperty("best cost REAL", [this] { return getBestCost(); });
92 addPlannerProgressProperty("milestone count INTEGER", [this] { return getMilestoneCountString(); });
93 addPlannerProgressProperty("edge count INTEGER", [this] { return getEdgeCountString(); });
94}
95
96ompl::geometric::PRM::PRM(const base::PlannerData &data, bool starStrategy)
97 : PRM(data.getSpaceInformation(), starStrategy)
98{
99 if (data.numVertices() > 0)
100 {
101 // mapping between vertex id from PlannerData and Vertex in Boost.Graph
102 std::map<unsigned int, Vertex> vertices;
103 // helper function to create vertices as needed and update the vertices mapping
104 const auto &getOrCreateVertex = [&](unsigned int vertex_index) {
105 if (!vertices.count(vertex_index))
106 {
107 const auto &data_vertex = data.getVertex(vertex_index);
108 Vertex graph_vertex = boost::add_vertex(g_);
109 stateProperty_[graph_vertex] = si_->cloneState(data_vertex.getState());
110 totalConnectionAttemptsProperty_[graph_vertex] = 1;
112 vertices[vertex_index] = graph_vertex;
113 }
114 return vertices.at(vertex_index);
115 };
116
117 specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
118 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
119 specs_.multithreaded = true;
120 nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
121
122 for (size_t vertex_index = 0; vertex_index < data.numVertices(); ++vertex_index)
123 {
124 Vertex m = getOrCreateVertex(vertex_index);
125 std::vector<unsigned int> neighbor_indices;
126 data.getEdges(vertex_index, neighbor_indices);
127 if (neighbor_indices.empty())
128 {
129 disjointSets_.make_set(m);
130 }
131 else
132 {
133 for (const unsigned int neighbor_index : neighbor_indices)
134 {
135 Vertex n = getOrCreateVertex(neighbor_index);
138 base::Cost weight;
139 data.getEdgeWeight(vertex_index, neighbor_index, &weight);
140 const Graph::edge_property_type properties(weight);
141 boost::add_edge(m, n, properties, g_);
142 uniteComponents(m, n);
143 }
144 }
145 nn_->add(m);
146 }
147 }
148}
149
150ompl::geometric::PRM::~PRM()
151{
152 freeMemory();
153}
154
156{
157 Planner::setup();
158 if (!nn_)
159 {
160 specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
161 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
162 specs_.multithreaded = true;
163 nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
164 }
165 if (!connectionStrategy_)
166 setDefaultConnectionStrategy();
167 if (!connectionFilter_)
168 connectionFilter_ = [](const Vertex &, const Vertex &) { return true; };
169
170 // Setup optimization objective
171 //
172 // If no optimization objective was specified, then default to
173 // optimizing path length as computed by the distance() function
174 // in the state space.
175 if (pdef_)
176 {
177 if (pdef_->hasOptimizationObjective())
178 opt_ = pdef_->getOptimizationObjective();
179 else
180 {
181 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
182 if (!starStrategy_)
183 opt_->setCostThreshold(opt_->infiniteCost());
184 }
185 }
186 else
187 {
188 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
189 setup_ = false;
190 }
191}
192
194{
195 if (starStrategy_)
196 throw Exception("Cannot set the maximum nearest neighbors for " + getName());
197 if (!nn_)
198 {
199 specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
200 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
201 specs_.multithreaded = true;
202 nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
203 }
204 if (!userSetConnectionStrategy_)
205 connectionStrategy_ = KStrategy<Vertex>(k, nn_);
206 if (isSetup())
207 setup();
208}
209
211{
212 const auto strategy = connectionStrategy_.target<KStrategy<Vertex>>();
213 return strategy ? strategy->getNumNeighbors() : 0u;
214}
215
217{
218 if (starStrategy_)
219 connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
220 else
221 connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
222}
223
224void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
225{
226 Planner::setProblemDefinition(pdef);
227 clearQuery();
228}
229
231{
232 startM_.clear();
233 goalM_.clear();
234 pis_.restart();
235}
236
238{
239 Planner::clear();
240 sampler_.reset();
241 simpleSampler_.reset();
242 freeMemory();
243 if (nn_)
244 nn_->clear();
245 clearQuery();
246
247 iterations_ = 0;
248 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
249}
250
252{
253 foreach (Vertex v, boost::vertices(g_))
254 si_->freeState(stateProperty_[v]);
255 g_.clear();
256}
257
259{
260 expandRoadmap(base::timedPlannerTerminationCondition(expandTime));
261}
262
264{
265 if (!simpleSampler_)
266 simpleSampler_ = si_->allocStateSampler();
267
268 std::vector<base::State *> states(magic::MAX_RANDOM_BOUNCE_STEPS);
269 si_->allocStates(states);
270 expandRoadmap(ptc, states);
271 si_->freeStates(states);
272}
273
275 std::vector<base::State *> &workStates)
276{
277 // construct a probability distribution over the vertices in the roadmap
278 // as indicated in
279 // "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
280 // Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
281
282 PDF<Vertex> pdf;
283 foreach (Vertex v, boost::vertices(g_))
284 {
285 const unsigned long int t = totalConnectionAttemptsProperty_[v];
286 pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
287 }
288
289 if (pdf.empty())
290 return;
291
292 while (!ptc)
293 {
294 iterations_++;
295 Vertex v = pdf.sample(rng_.uniform01());
296 unsigned int s =
297 si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
298 if (s > 0)
299 {
300 s--;
301 Vertex last = addMilestone(si_->cloneState(workStates[s]));
302
303 graphMutex_.lock();
304 for (unsigned int i = 0; i < s; ++i)
305 {
306 // add the vertex along the bouncing motion
307 Vertex m = boost::add_vertex(g_);
308 stateProperty_[m] = si_->cloneState(workStates[i]);
309 totalConnectionAttemptsProperty_[m] = 1;
310 successfulConnectionAttemptsProperty_[m] = 0;
311 disjointSets_.make_set(m);
312
313 // add the edge to the parent vertex
314 const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
315 const Graph::edge_property_type properties(weight);
316 boost::add_edge(v, m, properties, g_);
317 uniteComponents(v, m);
318
319 // add the vertex to the nearest neighbors data structure
320 nn_->add(m);
321 v = m;
322 }
323
324 // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
325 // we add an edge
326 if (s > 0 || !sameComponent(v, last))
327 {
328 // add the edge to the parent vertex
329 const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
330 const Graph::edge_property_type properties(weight);
331 boost::add_edge(v, last, properties, g_);
332 uniteComponents(v, last);
333 }
334 graphMutex_.unlock();
335 }
336 }
337}
338
340{
341 growRoadmap(base::timedPlannerTerminationCondition(growTime));
342}
343
345{
346 if (!isSetup())
347 setup();
348 if (!sampler_)
349 sampler_ = si_->allocValidStateSampler();
350
351 base::State *workState = si_->allocState();
352 growRoadmap(ptc, workState);
353 si_->freeState(workState);
354}
355
357{
358 /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
359 while (!ptc)
360 {
361 iterations_++;
362 // search for a valid state
363 bool found = false;
364 while (!found && !ptc)
365 {
366 unsigned int attempts = 0;
367 do
368 {
369 found = sampler_->sample(workState);
370 attempts++;
372 }
373 // add it as a milestone
374 if (found)
375 addMilestone(si_->cloneState(workState));
376 }
377}
378
380{
381 auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
382 while (!ptc && !addedNewSolution_)
383 {
384 // Check for any new goal states
385 if (goal->maxSampleCount() > goalM_.size())
386 {
387 const base::State *st = pis_.nextGoal();
388 if (st != nullptr)
389 goalM_.push_back(addMilestone(si_->cloneState(st)));
390 }
391
392 // Check for a solution
393 addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution);
394 // Sleep for 1ms
395 if (!addedNewSolution_)
396 std::this_thread::sleep_for(std::chrono::milliseconds(1));
397 }
398}
399
400bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
401 base::PathPtr &solution)
402{
403 base::Goal *g = pdef_->getGoal().get();
404 base::Cost sol_cost(opt_->infiniteCost());
405 foreach (Vertex start, starts)
406 {
407 foreach (Vertex goal, goals)
408 {
409 // we lock because the connected components algorithm is incremental and may change disjointSets_
410 graphMutex_.lock();
411 bool same_component = sameComponent(start, goal);
412 graphMutex_.unlock();
413
414 if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
415 {
416 base::PathPtr p = constructSolution(start, goal);
417 if (p)
418 {
419 base::Cost pathCost = p->cost(opt_);
420 if (opt_->isCostBetterThan(pathCost, bestCost_))
421 bestCost_ = pathCost;
422 // Check if optimization objective is satisfied
423 if (opt_->isSatisfied(pathCost))
424 {
425 solution = p;
426 return true;
427 }
428 if (opt_->isCostBetterThan(pathCost, sol_cost))
429 {
430 solution = p;
431 sol_cost = pathCost;
432 }
433 }
434 }
435 }
436 }
437
438 return false;
439}
440
442{
443 return addedNewSolution_;
444}
445
447{
448 checkValidity();
449 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
450
451 if (goal == nullptr)
452 {
453 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
455 }
456
457 // Add the valid start states as milestones
458 while (const base::State *st = pis_.nextStart())
459 startM_.push_back(addMilestone(si_->cloneState(st)));
460
461 if (startM_.empty())
462 {
463 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
465 }
466
467 if (!goal->couldSample())
468 {
469 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
471 }
472
473 // Ensure there is at least one valid goal state
474 if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
475 {
476 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
477 if (st != nullptr)
478 goalM_.push_back(addMilestone(si_->cloneState(st)));
479
480 if (goalM_.empty())
481 {
482 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
484 }
485 }
486
487 unsigned long int nrStartStates = boost::num_vertices(g_);
488 OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
489
490 // Reset addedNewSolution_ member and create solution checking thread
491 addedNewSolution_ = false;
492 base::PathPtr sol;
493 std::thread slnThread([this, &ptc, &sol] { checkForSolution(ptc, sol); });
494
495 // construct new planner termination condition that fires when the given ptc is true, or a solution is found
496 base::PlannerTerminationCondition ptcOrSolutionFound([this, &ptc] { return ptc || addedNewSolution(); });
497
498 constructRoadmap(ptcOrSolutionFound);
499
500 // Ensure slnThread is ceased before exiting solve
501 slnThread.join();
502
503 OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
504
505 if (sol)
506 {
507 base::PlannerSolution psol(sol);
508 psol.setPlannerName(getName());
509 // if the solution was optimized, we mark it as such
510 psol.setOptimized(opt_, bestCost_, addedNewSolution());
511 pdef_->addSolutionPath(psol);
512 }
513 else
514 {
515 // Return an approximate solution.
516 ompl::base::Cost diff = constructApproximateSolution(startM_, goalM_, sol);
517 if (!opt_->isFinite(diff))
518 {
519 OMPL_INFORM("Closest path is still start and goal");
521 }
522 OMPL_INFORM("Using approximate solution, heuristic cost-to-go is %f", diff.value());
523 pdef_->addSolutionPath(sol, true, diff.value(), getName());
525 }
526
528}
529
531{
532 if (!isSetup())
533 setup();
534 if (!sampler_)
535 sampler_ = si_->allocValidStateSampler();
536 if (!simpleSampler_)
537 simpleSampler_ = si_->allocStateSampler();
538
539 std::vector<base::State *> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
540 si_->allocStates(xstates);
541 bool grow = true;
542
543 bestCost_ = opt_->infiniteCost();
544 while (!ptc())
545 {
546 // maintain a 2:1 ratio for growing/expansion of roadmap
547 // call growRoadmap() twice as long for every call of expandRoadmap()
548 if (grow)
551 xstates[0]);
552 else
555 xstates);
556 grow = !grow;
557 }
558
559 si_->freeStates(xstates);
560}
561
563{
564 std::lock_guard<std::mutex> _(graphMutex_);
565
566 Vertex m = boost::add_vertex(g_);
567 stateProperty_[m] = state;
568 totalConnectionAttemptsProperty_[m] = 1;
569 successfulConnectionAttemptsProperty_[m] = 0;
570
571 // Initialize to its own (dis)connected component.
572 disjointSets_.make_set(m);
573
574 // Which milestones will we attempt to connect to?
575 const std::vector<Vertex> &neighbors = connectionStrategy_(m);
576
577 foreach (Vertex n, neighbors)
578 if (connectionFilter_(n, m))
579 {
580 totalConnectionAttemptsProperty_[m]++;
581 totalConnectionAttemptsProperty_[n]++;
582 if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
583 {
584 successfulConnectionAttemptsProperty_[m]++;
585 successfulConnectionAttemptsProperty_[n]++;
586 const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
587 const Graph::edge_property_type properties(weight);
588 boost::add_edge(n, m, properties, g_);
589 uniteComponents(n, m);
590 }
591 }
592
593 nn_->add(m);
594
595 return m;
596}
597
599{
600 disjointSets_.union_set(m1, m2);
601}
602
604{
605 return boost::same_component(m1, m2, disjointSets_);
606}
607
609 const std::vector<Vertex> &goals,
610 base::PathPtr &solution)
611{
612 std::lock_guard<std::mutex> _(graphMutex_);
613 base::Goal *g = pdef_->getGoal().get();
614 base::Cost closestVal(opt_->infiniteCost());
615 bool approxPathJustStart = true;
616
617 foreach (Vertex start, starts)
618 {
619 foreach (Vertex goal, goals)
620 {
621 base::Cost heuristicCost(costHeuristic(start, goal));
622 if (opt_->isCostBetterThan(heuristicCost, closestVal))
623 {
624 closestVal = heuristicCost;
625 approxPathJustStart = true;
626 }
627 if (!g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
628 {
629 continue;
630 }
631 base::PathPtr p;
632 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
633 boost::vector_property_map<base::Cost> dist(boost::num_vertices(g_));
634 boost::vector_property_map<base::Cost> rank(boost::num_vertices(g_));
635
636 try
637 {
638 // Consider using a persistent distance_map if it's slow
639 boost::astar_search(
640 g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
641 boost::predecessor_map(prev)
642 .distance_map(dist)
643 .rank_map(rank)
644 .distance_compare(
645 [this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
646 .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
647 .distance_inf(opt_->infiniteCost())
648 .distance_zero(opt_->identityCost())
649 .visitor(AStarGoalVisitor<Vertex>(goal)));
650 }
651 catch (AStarFoundGoal &)
652 {
653 }
654
655 Vertex closeToGoal = start;
656 for (auto vp = vertices(g_); vp.first != vp.second; vp.first++)
657 {
658 // We want to get the distance of each vertex to the goal.
659 // Boost lets us get cost-to-come, cost-to-come+dist-to-goal,
660 // but not just dist-to-goal.
661 ompl::base::Cost dist_to_goal(costHeuristic(*vp.first, goal));
662 if (opt_->isFinite(rank[*vp.first]) && opt_->isCostBetterThan(dist_to_goal, closestVal))
663 {
664 closeToGoal = *vp.first;
665 closestVal = dist_to_goal;
666 approxPathJustStart = false;
667 }
668 }
669 if (closeToGoal != start)
670 {
671 auto p(std::make_shared<PathGeometric>(si_));
672 for (Vertex pos = closeToGoal; prev[pos] != pos; pos = prev[pos])
673 p->append(stateProperty_[pos]);
674 p->append(stateProperty_[start]);
675 p->reverse();
676
677 solution = p;
678 }
679 }
680 }
681 if (approxPathJustStart)
682 {
683 return opt_->infiniteCost();
684 }
685 return closestVal;
686}
687
689{
690 std::lock_guard<std::mutex> _(graphMutex_);
691 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
692
693 try
694 {
695 // Consider using a persistent distance_map if it's slow
696 boost::astar_search(
697 g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
698 boost::predecessor_map(prev)
699 .distance_compare([this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
700 .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
701 .distance_inf(opt_->infiniteCost())
702 .distance_zero(opt_->identityCost())
703 .visitor(AStarGoalVisitor<Vertex>(goal)));
704 }
705 catch (AStarFoundGoal &)
706 {
707 }
708
709 if (prev[goal] == goal)
710 throw Exception(name_, "Could not find solution path");
711
712 auto p(std::make_shared<PathGeometric>(si_));
713 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
714 p->append(stateProperty_[pos]);
715 p->append(stateProperty_[start]);
716 p->reverse();
717
718 return p;
719}
720
722{
723 Planner::getPlannerData(data);
724
725 // Explicitly add start and goal states:
726 for (unsigned long i : startM_)
727 data.addStartVertex(
728 base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
729
730 for (unsigned long i : goalM_)
731 data.addGoalVertex(
732 base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
733
734 // Adding edges and all other vertices simultaneously
735 foreach (const Edge e, boost::edges(g_))
736 {
737 const Vertex v1 = boost::source(e, g_);
738 const Vertex v2 = boost::target(e, g_);
739 data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
740
741 // Add the reverse edge, since we're constructing an undirected roadmap
742 data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
743
744 // Add tags for the newly added vertices
745 data.tagState(stateProperty_[v1], const_cast<PRM *>(this)->disjointSets_.find_set(v1));
746 data.tagState(stateProperty_[v2], const_cast<PRM *>(this)->disjointSets_.find_set(v2));
747 }
748}
749
751{
752 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
753}
The exception type for ompl.
Definition Exception.h:47
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
virtual void add(const _T &data)=0
Add an element to the datastructure.
A container that supports probabilistic sampling over weighted data.
Definition PDF.h:49
bool empty() const
Returns whether the PDF contains no data.
Definition PDF.h:263
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition PDF.h:132
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element,...
Definition PDF.h:97
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
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
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,...
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
bool tagState(const State *st, int tag)
Set the integer tag associated with the given state. If the given state does not exist in a vertex,...
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...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)....
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
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...
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.
Probabilistic RoadMap planner.
Definition PRM.h:81
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 PRM.cpp:750
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition PRM.h:370
ompl::base::Cost constructApproximateSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
(Assuming that there is always an approximate solution), finds an approximate solution.
Definition PRM.cpp:608
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. Grows a roadmap using constructRoadmap()....
Definition PRM.cpp:446
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition PRM.cpp:441
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 PRM.h:405
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition PRM.cpp:230
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 PRM.cpp:603
void freeMemory()
Free all the memory allocated by the planner.
Definition PRM.cpp:251
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition PRM.h:379
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition PRM.h:127
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition PRM.h:125
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition PRM.cpp:72
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition PRM.cpp:379
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition PRM.cpp:258
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 PRM.h:345
bool maybeConstructSolution(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 PRM.cpp:400
base::PathPtr constructSolution(const Vertex &start, const Vertex &goal)
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition PRM.cpp:688
void uniteComponents(Vertex m1, Vertex m2)
Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...
Definition PRM.cpp:598
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition PRM.cpp:530
boost::property_map< Graph, vertex_successful_connection_attempts_t >::type successfulConnectionAttemptsProperty_
Access to the number of successful connection attempts for a vertex.
Definition PRM.h:398
void growRoadmap(double growTime)
If the user desires, the roadmap can be improved for the given time (seconds). The solve() method wil...
Definition PRM.cpp:339
Graph g_
Connectivity graph.
Definition PRM.h:382
Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition PRM.cpp:562
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition PRM.cpp:155
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition PRM.cpp:237
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition PRM.cpp:193
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 PRM.cpp:721
unsigned int getMaxNearestNeighbors() const
return the maximum number of nearest neighbors to connect a sample to
Definition PRM.cpp:210
void setDefaultConnectionStrategy()
Definition PRM.cpp:216
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition PRM.h:391
boost::property_map< Graph, vertex_total_connection_attempts_t >::type totalConnectionAttemptsProperty_
Access to the number of total connection attempts for a vertex.
Definition PRM.h:394
#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
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true,...
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
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 ...
static const double ROADMAP_BUILD_TIME
The time in seconds for a single roadmap building operation (dt)
Definition PRM.cpp:64
static const unsigned int DEFAULT_NEAREST_NEIGHBORS
The number of nearest neighbors to consider by default in the construction of the PRM roadmap.
Definition PRM.cpp:68
static const unsigned int MAX_RANDOM_BOUNCE_STEPS
The number of steps to take for a random bounce motion generated as part of the expansion step of PRM...
Definition PRM.cpp:61
Main namespace. Contains everything in this library.
Representation of a solution to a planning problem.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
Set the optimization objective used to optimize this solution, the cost of the solution and whether i...
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.
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
@ TIMEOUT
The planner failed to find a solution.