Loading...
Searching...
No Matches
LazyPRM.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2013, Willow Garage
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 Willow Garage 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, Ryan Luna, Henning Kayser */
36
37#include "ompl/geometric/planners/prm/LazyPRM.h"
38#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
39#include "ompl/base/goals/GoalSampleableRegion.h"
40#include "ompl/geometric/planners/prm/ConnectionStrategy.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/graph/lookup_edge.hpp>
45#include <boost/foreach.hpp>
46#include <queue>
47
48#include "GoalVisitor.hpp"
49
50#define foreach BOOST_FOREACH
51
52namespace ompl
53{
54 namespace magic
55 {
58 static const unsigned int DEFAULT_NEAREST_NEIGHBORS_LAZY = 5;
59
63 static const unsigned int MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION = 5;
64 }
65}
66
67ompl::geometric::LazyPRM::LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy)
68 : base::Planner(si, "LazyPRM")
69 , starStrategy_(starStrategy)
70 , indexProperty_(boost::get(boost::vertex_index_t(), g_))
71 , stateProperty_(boost::get(vertex_state_t(), g_))
72 , weightProperty_(boost::get(boost::edge_weight, g_))
73 , vertexComponentProperty_(boost::get(vertex_component_t(), g_))
74 , vertexValidityProperty_(boost::get(vertex_flags_t(), g_))
75 , edgeValidityProperty_(boost::get(edge_flags_t(), g_))
76{
80
81 Planner::declareParam<double>("range", this, &LazyPRM::setRange, &LazyPRM::getRange, "0.:1.:10000.");
82 if (!starStrategy_)
83 Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &LazyPRM::setMaxNearestNeighbors,
84 std::string("8:1000"));
85
86 addPlannerProgressProperty("iterations INTEGER", [this]
87 {
88 return getIterationCount();
89 });
90 addPlannerProgressProperty("best cost REAL", [this]
91 {
92 return getBestCost();
93 });
94 addPlannerProgressProperty("milestone count INTEGER", [this]
95 {
96 return getMilestoneCountString();
97 });
98 addPlannerProgressProperty("edge count INTEGER", [this]
99 {
100 return getEdgeCountString();
101 });
102}
103
105 : LazyPRM(data.getSpaceInformation(), starStrategy)
106{
107 if (data.numVertices() > 0)
108 {
109 // mapping between vertex id from PlannerData and Vertex in Boost.Graph
110 std::map<unsigned int, Vertex> vertices;
111 // helper function to create vertices as needed and update the vertices mapping
112 const auto &getOrCreateVertex = [&](unsigned int vertex_index) {
113 if (!vertices.count(vertex_index))
114 {
115 const auto &data_vertex = data.getVertex(vertex_index);
116 Vertex graph_vertex = boost::add_vertex(g_);
117 stateProperty_[graph_vertex] = si_->cloneState(data_vertex.getState());
119 unsigned long int newComponent = componentCount_++;
120 vertexComponentProperty_[graph_vertex] = newComponent;
121 vertices[vertex_index] = graph_vertex;
122 }
123 return vertices.at(vertex_index);
124 };
125
126 specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
127 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
128 specs_.multithreaded = true;
129 nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
130
131 for (size_t vertex_index = 0; vertex_index < data.numVertices(); ++vertex_index)
132 {
133 Vertex m = getOrCreateVertex(vertex_index);
134 std::vector<unsigned int> neighbor_indices;
135 data.getEdges(vertex_index, neighbor_indices);
136 for (const unsigned int neighbor_index : neighbor_indices)
137 {
138 Vertex n = getOrCreateVertex(neighbor_index);
139 base::Cost weight;
140 data.getEdgeWeight(vertex_index, neighbor_index, &weight);
141 const Graph::edge_property_type properties(weight);
142 const Edge &edge = boost::add_edge(m, n, properties, g_).first;
144 uniteComponents(m, n);
145 }
146 nn_->add(m);
147 }
148 }
149}
150
151ompl::geometric::LazyPRM::~LazyPRM() = default;
152
154{
155 Planner::setup();
156 tools::SelfConfig sc(si_, getName());
157 sc.configurePlannerRange(maxDistance_);
158
159 if (!nn_)
160 {
161 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
162 nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
163 {
164 return distanceFunction(a, b);
165 });
166 }
167 if (!connectionStrategy_)
168 setDefaultConnectionStrategy();
169 if (!connectionFilter_)
170 connectionFilter_ = [](const Vertex &, const Vertex &)
171 {
172 return true;
173 };
174
175 // Setup optimization objective
176 //
177 // If no optimization objective was specified, then default to
178 // optimizing path length as computed by the distance() function
179 // in the state space.
180 if (pdef_)
181 {
182 if (pdef_->hasOptimizationObjective())
183 opt_ = pdef_->getOptimizationObjective();
184 else
185 {
186 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
187 if (!starStrategy_)
188 opt_->setCostThreshold(opt_->infiniteCost());
189 }
190 }
191 else
192 {
193 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
194 setup_ = false;
195 }
196
197 sampler_ = si_->allocStateSampler();
198}
199
201{
202 maxDistance_ = distance;
203 if (!userSetConnectionStrategy_)
204 setDefaultConnectionStrategy();
205 if (isSetup())
206 setup();
207}
208
210{
211 if (starStrategy_)
212 throw Exception("Cannot set the maximum nearest neighbors for " + getName());
213 if (!nn_)
214 {
215 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
216 nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
217 {
218 return distanceFunction(a, b);
219 });
220 }
221 if (!userSetConnectionStrategy_)
222 connectionStrategy_ = KBoundedStrategy<Vertex>(k, maxDistance_, nn_);
223 if (isSetup())
224 setup();
225}
226
228{
229 if (starStrategy_)
230 connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
231 else
232 connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
233}
234
235void ompl::geometric::LazyPRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
236{
237 Planner::setProblemDefinition(pdef);
238 clearQuery();
239}
240
242{
243 startM_.clear();
244 goalM_.clear();
245 pis_.restart();
246}
247
249{
250 foreach (const Vertex v, boost::vertices(g_))
251 vertexValidityProperty_[v] = VALIDITY_UNKNOWN;
252 foreach (const Edge e, boost::edges(g_))
253 edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
254}
255
257{
258 Planner::clear();
259 freeMemory();
260 if (nn_)
261 nn_->clear();
262 clearQuery();
263
264 componentCount_ = 0;
265 iterations_ = 0;
266 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
267}
268
270{
271 foreach (Vertex v, boost::vertices(g_))
272 si_->freeState(stateProperty_[v]);
273 g_.clear();
274}
275
277{
278 Vertex m = boost::add_vertex(g_);
279 stateProperty_[m] = state;
280 vertexValidityProperty_[m] = VALIDITY_UNKNOWN;
281 unsigned long int newComponent = componentCount_++;
282 vertexComponentProperty_[m] = newComponent;
283 componentSize_[newComponent] = 1;
284
285 // Which milestones will we attempt to connect to?
286 const std::vector<Vertex> &neighbors = connectionStrategy_(m);
287 foreach (Vertex n, neighbors)
288 if (connectionFilter_(m, n))
289 {
290 const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
291 const Graph::edge_property_type properties(weight);
292 const Edge &e = boost::add_edge(m, n, properties, g_).first;
293 edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
294 uniteComponents(m, n);
295 }
296
297 nn_->add(m);
298
299 return m;
300}
301
303{
304 checkValidity();
305 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
306
307 if (goal == nullptr)
308 {
309 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
311 }
312
313 // Add the valid start states as milestones
314 while (const base::State *st = pis_.nextStart())
315 startM_.push_back(addMilestone(si_->cloneState(st)));
316
317 if (startM_.empty())
318 {
319 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
321 }
322
323 if (!goal->couldSample())
324 {
325 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
327 }
328
329 // Ensure there is at least one valid goal state
330 if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
331 {
332 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
333 if (st != nullptr)
334 goalM_.push_back(addMilestone(si_->cloneState(st)));
335
336 if (goalM_.empty())
337 {
338 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
340 }
341 }
342
343 unsigned long int nrStartStates = boost::num_vertices(g_);
344 OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
345
346 bestCost_ = opt_->infiniteCost();
347 base::State *workState = si_->allocState();
348 std::pair<std::size_t, std::size_t> startGoalPair;
349 base::PathPtr bestSolution;
350 bool fullyOptimized = false;
351 bool someSolutionFound = false;
352 unsigned int optimizingComponentSegments = 0;
353
354 // Grow roadmap in lazy fashion -- add vertices and edges without checking validity
355 while (!ptc)
356 {
357 ++iterations_;
358 sampler_->sampleUniform(workState);
359 Vertex addedVertex = addMilestone(si_->cloneState(workState));
360
361 const long int solComponent = solutionComponent(&startGoalPair);
362 // If the start & goal are connected and we either did not find any solution
363 // so far or the one we found still needs optimizing and we just added an edge
364 // to the connected component that is used for the solution, we attempt to
365 // construct a new solution.
366 if (solComponent != -1 &&
367 (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent))
368 {
369 // If we already have a solution, we are optimizing. We check that we added at least
370 // a few segments to the connected component that includes the previously found
371 // solution before attempting to construct a new solution.
372 if (someSolutionFound)
373 {
374 if (++optimizingComponentSegments < magic::MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION)
375 continue;
376 optimizingComponentSegments = 0;
377 }
378 Vertex startV = startM_[startGoalPair.first];
379 Vertex goalV = goalM_[startGoalPair.second];
380 base::PathPtr solution;
381 do
382 {
383 solution = constructSolution(startV, goalV);
384 } while (!solution && vertexComponentProperty_[startV] == vertexComponentProperty_[goalV]);
385 if (solution)
386 {
387 someSolutionFound = true;
388 base::Cost c = solution->cost(opt_);
389 if (opt_->isSatisfied(c))
390 {
391 fullyOptimized = true;
392 bestSolution = solution;
393 bestCost_ = c;
394 break;
395 }
396 if (opt_->isCostBetterThan(c, bestCost_))
397 {
398 bestSolution = solution;
399 bestCost_ = c;
400 }
401 }
402 }
403 }
404
405 si_->freeState(workState);
406
407 if (bestSolution)
408 {
409 base::PlannerSolution psol(bestSolution);
410 psol.setPlannerName(getName());
411 // if the solution was optimized, we mark it as such
412 psol.setOptimized(opt_, bestCost_, fullyOptimized);
413 pdef_->addSolutionPath(psol);
414 }
415
416 OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
417
419}
420
421void ompl::geometric::LazyPRM::uniteComponents(Vertex a, Vertex b)
422{
423 unsigned long int componentA = vertexComponentProperty_[a];
424 unsigned long int componentB = vertexComponentProperty_[b];
425 if (componentA == componentB)
426 return;
427 if (componentSize_[componentA] > componentSize_[componentB])
428 {
429 std::swap(componentA, componentB);
430 std::swap(a, b);
431 }
432 markComponent(a, componentB);
433}
434
435void ompl::geometric::LazyPRM::markComponent(Vertex v, unsigned long int newComponent)
436{
437 std::queue<Vertex> q;
438 q.push(v);
439 while (!q.empty())
440 {
441 Vertex n = q.front();
442 q.pop();
443 unsigned long int &component = vertexComponentProperty_[n];
444 if (component == newComponent)
445 continue;
446 if (componentSize_[component] == 1)
447 componentSize_.erase(component);
448 else
449 componentSize_[component]--;
450 component = newComponent;
451 componentSize_[newComponent]++;
452 boost::graph_traits<Graph>::adjacency_iterator nbh, last;
453 for (boost::tie(nbh, last) = boost::adjacent_vertices(n, g_); nbh != last; ++nbh)
454 q.push(*nbh);
455 }
456}
457
458long int ompl::geometric::LazyPRM::solutionComponent(std::pair<std::size_t, std::size_t> *startGoalPair) const
459{
460 for (std::size_t startIndex = 0; startIndex < startM_.size(); ++startIndex)
461 {
462 long int startComponent = vertexComponentProperty_[startM_[startIndex]];
463 for (std::size_t goalIndex = 0; goalIndex < goalM_.size(); ++goalIndex)
464 {
465 if (startComponent == (long int)vertexComponentProperty_[goalM_[goalIndex]])
466 {
467 startGoalPair->first = startIndex;
468 startGoalPair->second = goalIndex;
469 return startComponent;
470 }
471 }
472 }
473 return -1;
474}
475
477{
478 // Need to update the index map here, becuse nodes may have been removed and
479 // the numbering will not be 0 .. N-1 otherwise.
480 unsigned long int index = 0;
481 boost::graph_traits<Graph>::vertex_iterator vi, vend;
482 for (boost::tie(vi, vend) = boost::vertices(g_); vi != vend; ++vi, ++index)
483 indexProperty_[*vi] = index;
484
485 boost::property_map<Graph, boost::vertex_predecessor_t>::type prev;
486 try
487 {
488 // Consider using a persistent distance_map if it's slow
489 boost::astar_search(g_, start,
490 [this, goal](Vertex v)
491 {
492 return costHeuristic(v, goal);
493 },
494 boost::predecessor_map(prev)
495 .distance_compare([this](base::Cost c1, base::Cost c2)
496 {
497 return opt_->isCostBetterThan(c1, c2);
498 })
499 .distance_combine([this](base::Cost c1, base::Cost c2)
500 {
501 return opt_->combineCosts(c1, c2);
502 })
503 .distance_inf(opt_->infiniteCost())
504 .distance_zero(opt_->identityCost())
505 .visitor(AStarGoalVisitor<Vertex>(goal)));
506 }
507 catch (AStarFoundGoal &)
508 {
509 }
510 if (prev[goal] == goal)
511 throw Exception(name_, "Could not find solution path");
512
513 // First, get the solution states without copying them, and check them for validity.
514 // We do all the node validity checks for the vertices, as this may remove a larger
515 // part of the graph (compared to removing an edge).
516 std::vector<const base::State *> states(1, stateProperty_[goal]);
517 std::set<Vertex> milestonesToRemove;
518 for (Vertex pos = prev[goal]; prev[pos] != pos; pos = prev[pos])
519 {
520 const base::State *st = stateProperty_[pos];
521 unsigned int &vd = vertexValidityProperty_[pos];
522 if ((vd & VALIDITY_TRUE) == 0)
523 if (si_->isValid(st))
524 vd |= VALIDITY_TRUE;
525 if ((vd & VALIDITY_TRUE) == 0)
526 milestonesToRemove.insert(pos);
527 if (milestonesToRemove.empty())
528 states.push_back(st);
529 }
530
531 // We remove *all* invalid vertices. This is not entirely as described in the original LazyPRM
532 // paper, as the paper suggest removing the first vertex only, and then recomputing the
533 // shortest path. Howeve, the paper says the focus is on efficient vertex & edge removal,
534 // rather than collision checking, so this modification is in the spirit of the paper.
535 if (!milestonesToRemove.empty())
536 {
537 unsigned long int comp = vertexComponentProperty_[start];
538 // Remember the current neighbors.
539 std::set<Vertex> neighbors;
540 for (auto it = milestonesToRemove.begin(); it != milestonesToRemove.end(); ++it)
541 {
542 boost::graph_traits<Graph>::adjacency_iterator nbh, last;
543 for (boost::tie(nbh, last) = boost::adjacent_vertices(*it, g_); nbh != last; ++nbh)
544 if (milestonesToRemove.find(*nbh) == milestonesToRemove.end())
545 neighbors.insert(*nbh);
546 // Remove vertex from nearest neighbors data structure.
547 nn_->remove(*it);
548 // Free vertex state.
549 si_->freeState(stateProperty_[*it]);
550 // Remove all edges.
551 boost::clear_vertex(*it, g_);
552 // Remove the vertex.
553 boost::remove_vertex(*it, g_);
554 }
555 // Update the connected component ID for neighbors.
556 for (auto neighbor : neighbors)
557 {
558 if (comp == vertexComponentProperty_[neighbor])
559 {
560 unsigned long int newComponent = componentCount_++;
561 componentSize_[newComponent] = 0;
562 markComponent(neighbor, newComponent);
563 }
564 }
565 return base::PathPtr();
566 }
567
568 // start is checked for validity already
569 states.push_back(stateProperty_[start]);
570
571 // Check the edges too, if the vertices were valid. Remove the first invalid edge only.
572 std::vector<const base::State *>::const_iterator prevState = states.begin(), state = prevState + 1;
573 Vertex prevVertex = goal, pos = prev[goal];
574 do
575 {
576 Edge e = boost::lookup_edge(pos, prevVertex, g_).first;
577 unsigned int &evd = edgeValidityProperty_[e];
578 if ((evd & VALIDITY_TRUE) == 0)
579 {
580 if (si_->checkMotion(*state, *prevState))
581 evd |= VALIDITY_TRUE;
582 }
583 if ((evd & VALIDITY_TRUE) == 0)
584 {
585 boost::remove_edge(e, g_);
586 unsigned long int newComponent = componentCount_++;
587 componentSize_[newComponent] = 0;
588 markComponent(pos, newComponent);
589 return base::PathPtr();
590 }
591 prevState = state;
592 ++state;
593 prevVertex = pos;
594 pos = prev[pos];
595 } while (prevVertex != pos);
596
597 auto p(std::make_shared<PathGeometric>(si_));
598 for (std::vector<const base::State *>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
599 p->append(*st);
600 return p;
601}
602
604{
605 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
606}
607
609{
610 Planner::getPlannerData(data);
611
612 // Explicitly add start and goal states. Tag all states known to be valid as 1.
613 // Unchecked states are tagged as 0.
614 for (auto i : startM_)
615 data.addStartVertex(base::PlannerDataVertex(stateProperty_[i], 1));
616
617 for (auto i : goalM_)
618 data.addGoalVertex(base::PlannerDataVertex(stateProperty_[i], 1));
619
620 // Adding edges and all other vertices simultaneously
621 foreach (const Edge e, boost::edges(g_))
622 {
623 const Vertex v1 = boost::source(e, g_);
624 const Vertex v2 = boost::target(e, g_);
625 data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
626
627 // Add the reverse edge, since we're constructing an undirected roadmap
628 data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
629
630 // Add tags for the newly added vertices
631 data.tagState(stateProperty_[v1], (vertexValidityProperty_[v1] & VALIDITY_TRUE) == 0 ? 0 : 1);
632 data.tagState(stateProperty_[v2], (vertexValidityProperty_[v2] & VALIDITY_TRUE) == 0 ? 0 : 1);
633 }
634}
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.
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.
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: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
Return at most k neighbors, as long as they are also within a specified bound.
Make the minimal number of connections required to ensure asymptotic optimality.
Lazy Probabilistic RoadMap planner.
Definition LazyPRM.h:74
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition LazyPRM.cpp:241
static const unsigned int VALIDITY_UNKNOWN
Flag indicating validity of an edge of a vertex.
Definition LazyPRM.h:254
void clearValidity()
change the validity flag of each node and edge to VALIDITY_UNKNOWN
Definition LazyPRM.cpp:248
void setRange(double distance)
Set the maximum length of a motion to be added to the roadmap.
Definition LazyPRM.cpp:200
unsigned long int componentCount_
Number of connected components created so far. This is used as an ID only, does not represent the act...
Definition LazyPRM.h:360
void freeMemory()
Free all the memory allocated by the planner.
Definition LazyPRM.cpp:269
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition LazyPRM.cpp:209
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 LazyPRM.cpp:603
ompl::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 LazyPRM.cpp:476
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition LazyPRM.cpp:256
boost::property_map< Graph, vertex_component_t >::type vertexComponentProperty_
Access the connected component of a vertex.
Definition LazyPRM.h:350
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 LazyPRM.cpp:302
Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition LazyPRM.cpp:276
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition LazyPRM.h:310
Graph g_
Connectivity graph.
Definition LazyPRM.h:332
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 LazyPRM.cpp:608
boost::adjacency_list_traits< boost::vecS, boost::listS, boost::undirectedS >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition LazyPRM.h:97
boost::property_map< Graph, vertex_flags_t >::type vertexValidityProperty_
Access the validity state of a vertex.
Definition LazyPRM.h:353
double getRange() const
Get the range the planner is using.
Definition LazyPRM.h:158
LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition LazyPRM.cpp:67
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition LazyPRM.h:329
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition LazyPRM.h:344
void setDefaultConnectionStrategy()
Definition LazyPRM.cpp:227
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 LazyPRM.h:300
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition LazyPRM.h:130
long int solutionComponent(std::pair< std::size_t, std::size_t > *startGoalPair) const
Check if any pair of a start state and goal state are part of the same connected component....
Definition LazyPRM.cpp:458
boost::property_map< Graph, edge_flags_t >::type edgeValidityProperty_
Access the validity state of an edge.
Definition LazyPRM.h:356
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition LazyPRM.cpp:153
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#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
static const unsigned int DEFAULT_NEAREST_NEIGHBORS_LAZY
The number of nearest neighbors to consider by default in the construction of the PRM roadmap.
Definition LazyPRM.cpp:58
static const unsigned int MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION
When optimizing solutions with lazy planners, this is the minimum number of path segments to add befo...
Definition LazyPRM.cpp:63
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: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.