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());
118 vertexValidityProperty_[graph_vertex] = VALIDITY_UNKNOWN;
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
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()
152{
153 clear();
154};
155
157{
158 Planner::setup();
159 tools::SelfConfig sc(si_, getName());
160 sc.configurePlannerRange(maxDistance_);
161
162 if (!nn_)
163 {
165 nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
166 {
167 return distanceFunction(a, b);
168 });
169 }
170 if (!connectionStrategy_)
171 setDefaultConnectionStrategy();
172 if (!connectionFilter_)
173 connectionFilter_ = [](const Vertex &, const Vertex &)
174 {
175 return true;
176 };
177
178 // Setup optimization objective
179 //
180 // If no optimization objective was specified, then default to
181 // optimizing path length as computed by the distance() function
182 // in the state space.
183 if (pdef_)
184 {
185 if (pdef_->hasOptimizationObjective())
186 opt_ = pdef_->getOptimizationObjective();
187 else
188 {
189 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
190 if (!starStrategy_)
191 opt_->setCostThreshold(opt_->infiniteCost());
192 }
193 }
194 else
195 {
196 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
197 setup_ = false;
198 }
199
200 sampler_ = si_->allocStateSampler();
201}
202
204{
205 maxDistance_ = distance;
206 if (!userSetConnectionStrategy_)
207 setDefaultConnectionStrategy();
208 if (isSetup())
209 setup();
210}
211
213{
214 if (starStrategy_)
215 throw Exception("Cannot set the maximum nearest neighbors for " + getName());
216 if (!nn_)
217 {
219 nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
220 {
221 return distanceFunction(a, b);
222 });
223 }
224 if (!userSetConnectionStrategy_)
225 connectionStrategy_ = KBoundedStrategy<Vertex>(k, maxDistance_, nn_);
226 if (isSetup())
227 setup();
228}
229
231{
232 if (!nn_)
233 {
235 nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
236 {
237 return distanceFunction(a, b);
238 });
239 }
240
241 if (starStrategy_)
242 connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
243 else
244 connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
245}
246
247void ompl::geometric::LazyPRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
248{
249 Planner::setProblemDefinition(pdef);
250 clearQuery();
251}
252
254{
255 startM_.clear();
256 goalM_.clear();
257 pis_.restart();
258}
259
261{
262 foreach (const Vertex v, boost::vertices(g_))
263 vertexValidityProperty_[v] = VALIDITY_UNKNOWN;
264 foreach (const Edge e, boost::edges(g_))
265 edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
266}
267
269{
270 Planner::clear();
271 freeMemory();
272 if (nn_)
273 nn_->clear();
274 clearQuery();
275
276 componentCount_ = 0;
277 iterations_ = 0;
278 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
279}
280
282{
283 foreach (Vertex v, boost::vertices(g_))
284 si_->freeState(stateProperty_[v]);
285 g_.clear();
286}
287
289{
290 Vertex m = boost::add_vertex(g_);
291 stateProperty_[m] = state;
292 vertexValidityProperty_[m] = VALIDITY_UNKNOWN;
293 unsigned long int newComponent = componentCount_++;
294 vertexComponentProperty_[m] = newComponent;
295 componentSize_[newComponent] = 1;
296
297 // Which milestones will we attempt to connect to?
298 const std::vector<Vertex> &neighbors = connectionStrategy_(m);
299 foreach (Vertex n, neighbors)
300 if (connectionFilter_(m, n))
301 {
302 const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
303 const Graph::edge_property_type properties(weight);
304 const Edge &e = boost::add_edge(m, n, properties, g_).first;
305 edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
306 uniteComponents(m, n);
307 }
308
309 nn_->add(m);
310
311 return m;
312}
313
315{
316 checkValidity();
317 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
318
319 if (goal == nullptr)
320 {
321 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
323 }
324
325 // Add the valid start states as milestones
326 while (const base::State *st = pis_.nextStart())
327 startM_.push_back(addMilestone(si_->cloneState(st)));
328
329 if (startM_.empty())
330 {
331 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
333 }
334
335 if (!goal->couldSample())
336 {
337 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
339 }
340
341 // Ensure there is at least one valid goal state
342 if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
343 {
344 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
345 if (st != nullptr)
346 goalM_.push_back(addMilestone(si_->cloneState(st)));
347
348 if (goalM_.empty())
349 {
350 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
352 }
353 }
354
355 unsigned long int nrStartStates = boost::num_vertices(g_);
356 OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
357
358 bestCost_ = opt_->infiniteCost();
359 base::State *workState = si_->allocState();
360 std::pair<std::size_t, std::size_t> startGoalPair;
361 base::PathPtr bestSolution;
362 bool fullyOptimized = false;
363 bool someSolutionFound = false;
364 unsigned int optimizingComponentSegments = 0;
365
366 // Grow roadmap in lazy fashion -- add vertices and edges without checking validity
367 while (!ptc)
368 {
369 ++iterations_;
370 sampler_->sampleUniform(workState);
371 Vertex addedVertex = addMilestone(si_->cloneState(workState));
372
373 const long int solComponent = solutionComponent(&startGoalPair);
374 // If the start & goal are connected and we either did not find any solution
375 // so far or the one we found still needs optimizing and we just added an edge
376 // to the connected component that is used for the solution, we attempt to
377 // construct a new solution.
378 if (solComponent != -1 &&
379 (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent))
380 {
381 // If we already have a solution, we are optimizing. We check that we added at least
382 // a few segments to the connected component that includes the previously found
383 // solution before attempting to construct a new solution.
384 if (someSolutionFound)
385 {
386 if (++optimizingComponentSegments < magic::MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION)
387 continue;
388 optimizingComponentSegments = 0;
389 }
390 Vertex startV = startM_[startGoalPair.first];
391 Vertex goalV = goalM_[startGoalPair.second];
392 base::PathPtr solution;
393 do
394 {
395 solution = constructSolution(startV, goalV);
396 } while (!solution && vertexComponentProperty_[startV] == vertexComponentProperty_[goalV] && !ptc);
397 if (solution)
398 {
399 someSolutionFound = true;
400 base::Cost c = solution->cost(opt_);
401 if (opt_->isSatisfied(c))
402 {
403 fullyOptimized = true;
404 bestSolution = solution;
405 bestCost_ = c;
406 break;
407 }
408 if (opt_->isCostBetterThan(c, bestCost_))
409 {
410 bestSolution = solution;
411 bestCost_ = c;
412 }
413 }
414 }
415 }
416
417 si_->freeState(workState);
418
419 if (bestSolution)
420 {
421 base::PlannerSolution psol(bestSolution);
422 psol.setPlannerName(getName());
423 // if the solution was optimized, we mark it as such
424 psol.setOptimized(opt_, bestCost_, fullyOptimized);
425 pdef_->addSolutionPath(psol);
426 }
427
428 OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
429
431}
432
433void ompl::geometric::LazyPRM::uniteComponents(Vertex a, Vertex b)
434{
435 unsigned long int componentA = vertexComponentProperty_[a];
436 unsigned long int componentB = vertexComponentProperty_[b];
437 if (componentA == componentB)
438 return;
439 if (componentSize_[componentA] > componentSize_[componentB])
440 {
441 std::swap(componentA, componentB);
442 std::swap(a, b);
443 }
444 markComponent(a, componentB);
445}
446
447void ompl::geometric::LazyPRM::markComponent(Vertex v, unsigned long int newComponent)
448{
449 std::queue<Vertex> q;
450 q.push(v);
451 while (!q.empty())
452 {
453 Vertex n = q.front();
454 q.pop();
455 unsigned long int &component = vertexComponentProperty_[n];
456 if (component == newComponent)
457 continue;
458 if (componentSize_[component] == 1)
459 componentSize_.erase(component);
460 else
461 componentSize_[component]--;
462 component = newComponent;
463 componentSize_[newComponent]++;
464 boost::graph_traits<Graph>::adjacency_iterator nbh, last;
465 for (boost::tie(nbh, last) = boost::adjacent_vertices(n, g_); nbh != last; ++nbh)
466 q.push(*nbh);
467 }
468}
469
470long int ompl::geometric::LazyPRM::solutionComponent(std::pair<std::size_t, std::size_t> *startGoalPair) const
471{
472 for (std::size_t startIndex = 0; startIndex < startM_.size(); ++startIndex)
473 {
474 long int startComponent = vertexComponentProperty_[startM_[startIndex]];
475 for (std::size_t goalIndex = 0; goalIndex < goalM_.size(); ++goalIndex)
476 {
477 if (startComponent == (long int)vertexComponentProperty_[goalM_[goalIndex]])
478 {
479 startGoalPair->first = startIndex;
480 startGoalPair->second = goalIndex;
481 return startComponent;
482 }
483 }
484 }
485 return -1;
486}
487
489{
490 // Need to update the index map here, becuse nodes may have been removed and
491 // the numbering will not be 0 .. N-1 otherwise.
492 unsigned long int index = 0;
493 boost::graph_traits<Graph>::vertex_iterator vi, vend;
494 for (boost::tie(vi, vend) = boost::vertices(g_); vi != vend; ++vi, ++index)
495 indexProperty_[*vi] = index;
496
497 boost::property_map<Graph, boost::vertex_predecessor_t>::type prev;
498 try
499 {
500 // Consider using a persistent distance_map if it's slow
501 boost::astar_search(g_, start,
502 [this, goal](Vertex v)
503 {
504 return costHeuristic(v, goal);
505 },
506 boost::predecessor_map(prev)
507 .distance_compare([this](base::Cost c1, base::Cost c2)
508 {
509 return opt_->isCostBetterThan(c1, c2);
510 })
511 .distance_combine([this](base::Cost c1, base::Cost c2)
512 {
513 return opt_->combineCosts(c1, c2);
514 })
515 .distance_inf(opt_->infiniteCost())
516 .distance_zero(opt_->identityCost())
517 .visitor(AStarGoalVisitor<Vertex>(goal)));
518 }
519 catch (AStarFoundGoal &)
520 {
521 }
522 if (prev[goal] == goal)
523 throw Exception(name_, "Could not find solution path");
524
525 // First, get the solution states without copying them, and check them for validity.
526 // We do all the node validity checks for the vertices, as this may remove a larger
527 // part of the graph (compared to removing an edge).
528 std::vector<const base::State *> states(1, stateProperty_[goal]);
529 std::set<Vertex> milestonesToRemove;
530 for (Vertex pos = prev[goal]; prev[pos] != pos; pos = prev[pos])
531 {
532 const base::State *st = stateProperty_[pos];
533 unsigned int &vd = vertexValidityProperty_[pos];
534 if ((vd & VALIDITY_TRUE) == 0)
535 if (si_->isValid(st))
536 vd |= VALIDITY_TRUE;
537 if ((vd & VALIDITY_TRUE) == 0)
538 milestonesToRemove.insert(pos);
539 if (milestonesToRemove.empty())
540 states.push_back(st);
541 }
542
543 // We remove *all* invalid vertices. This is not entirely as described in the original LazyPRM
544 // paper, as the paper suggest removing the first vertex only, and then recomputing the
545 // shortest path. Howeve, the paper says the focus is on efficient vertex & edge removal,
546 // rather than collision checking, so this modification is in the spirit of the paper.
547 if (!milestonesToRemove.empty())
548 {
549 unsigned long int comp = vertexComponentProperty_[start];
550 // Remember the current neighbors.
551 std::set<Vertex> neighbors;
552 for (auto it = milestonesToRemove.begin(); it != milestonesToRemove.end(); ++it)
553 {
554 boost::graph_traits<Graph>::adjacency_iterator nbh, last;
555 for (boost::tie(nbh, last) = boost::adjacent_vertices(*it, g_); nbh != last; ++nbh)
556 if (milestonesToRemove.find(*nbh) == milestonesToRemove.end())
557 neighbors.insert(*nbh);
558 // Remove vertex from nearest neighbors data structure.
559 nn_->remove(*it);
560 // Free vertex state.
561 si_->freeState(stateProperty_[*it]);
562 // Remove all edges.
563 boost::clear_vertex(*it, g_);
564 // Remove the vertex.
565 boost::remove_vertex(*it, g_);
566 }
567 // Update the connected component ID for neighbors.
568 for (auto neighbor : neighbors)
569 {
570 if (comp == vertexComponentProperty_[neighbor])
571 {
572 unsigned long int newComponent = componentCount_++;
573 componentSize_[newComponent] = 0;
574 markComponent(neighbor, newComponent);
575 }
576 }
577 return base::PathPtr();
578 }
579
580 // start is checked for validity already
581 states.push_back(stateProperty_[start]);
582
583 // Check the edges too, if the vertices were valid. Remove the first invalid edge only.
584 std::vector<const base::State *>::const_iterator prevState = states.begin(), state = prevState + 1;
585 Vertex prevVertex = goal, pos = prev[goal];
586 do
587 {
588 Edge e = boost::lookup_edge(pos, prevVertex, g_).first;
589 unsigned int &evd = edgeValidityProperty_[e];
590 if ((evd & VALIDITY_TRUE) == 0)
591 {
592 if (si_->checkMotion(*state, *prevState))
593 evd |= VALIDITY_TRUE;
594 }
595 if ((evd & VALIDITY_TRUE) == 0)
596 {
597 boost::remove_edge(e, g_);
598 unsigned long int newComponent = componentCount_++;
599 componentSize_[newComponent] = 0;
600 markComponent(pos, newComponent);
601 return base::PathPtr();
602 }
603 prevState = state;
604 ++state;
605 prevVertex = pos;
606 pos = prev[pos];
607 } while (prevVertex != pos);
608
609 auto p(std::make_shared<PathGeometric>(si_));
610 for (std::vector<const base::State *>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
611 p->append(*st);
612 return p;
613}
614
616{
617 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
618}
619
621{
622 Planner::getPlannerData(data);
623
624 // Explicitly add start and goal states. Tag all states known to be valid as 1.
625 // Unchecked states are tagged as 0.
626 for (auto i : startM_)
627 data.addStartVertex(base::PlannerDataVertex(stateProperty_[i], 1));
628
629 for (auto i : goalM_)
630 data.addGoalVertex(base::PlannerDataVertex(stateProperty_[i], 1));
631
632 // Adding edges and all other vertices simultaneously
633 foreach (const Edge e, boost::edges(g_))
634 {
635 const Vertex v1 = boost::source(e, g_);
636 const Vertex v2 = boost::target(e, g_);
637 data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
638
639 // Add the reverse edge, since we're constructing an undirected roadmap
640 data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
641
642 // Add tags for the newly added vertices
643 data.tagState(stateProperty_[v1], (vertexValidityProperty_[v1] & VALIDITY_TRUE) == 0 ? 0 : 1);
644 data.tagState(stateProperty_[v2], (vertexValidityProperty_[v2] & VALIDITY_TRUE) == 0 ? 0 : 1);
645 }
646}
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.
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)....
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
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:253
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:260
void setRange(double distance)
Set the maximum length of a motion to be added to the roadmap.
Definition LazyPRM.cpp:203
void freeMemory()
Free all the memory allocated by the planner.
Definition LazyPRM.cpp:281
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition LazyPRM.cpp:212
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:615
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:488
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition LazyPRM.cpp:268
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:314
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:288
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:620
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
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
void setDefaultConnectionStrategy()
Definition LazyPRM.cpp:230
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:470
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:156
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
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: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.