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/lambda/bind.hpp>
45 #include <boost/graph/astar_search.hpp>
46 #include <boost/graph/incremental_components.hpp>
47 #include <boost/property_map/vector_property_map.hpp>
48 #include <boost/foreach.hpp>
49 #include <boost/thread.hpp>
51 #include "GoalVisitor.hpp"
53 #define foreach BOOST_FOREACH
54 #define foreach_reverse BOOST_REVERSE_FOREACH
75 base::Planner(si,
"PRM"),
76 starStrategy_(starStrategy),
80 weightProperty_(boost::get(boost::edge_weight, g_)),
81 edgeIDProperty_(boost::get(boost::edge_index, g_)),
82 disjointSets_(boost::get(boost::vertex_rank, g_),
83 boost::get(boost::vertex_predecessor, g_)),
85 userSetConnectionStrategy_(false),
95 ompl::geometric::PRM::~PRM(
void)
105 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
108 if (!connectionStrategy_)
115 if (!connectionFilter_)
116 connectionFilter_ = boost::lambda::constant(
true);
118 if (pdef_->hasOptimizationObjective())
119 opt_ = pdef_->getOptimizationObjective();
123 opt_->setCostThreshold(opt_->infiniteCost());
131 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
139 Planner::setProblemDefinition(pdef);
154 simpleSampler_.reset();
164 foreach (Vertex v, boost::vertices(g_))
165 si_->freeState(stateProperty_[v]);
177 simpleSampler_ = si_->allocStateSampler();
180 si_->allocStates(states);
181 expandRoadmap(ptc, states);
182 si_->freeStates(states);
186 std::vector<base::State*> &workStates)
194 foreach (Vertex v, boost::vertices(g_))
196 const unsigned int t = totalConnectionAttemptsProperty_[v];
197 pdf.
add(v, (
double)(t - successfulConnectionAttemptsProperty_[v]) / (
double)t);
205 Vertex v = pdf.
sample(rng_.uniform01());
206 unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates,
false);
210 Vertex last = addMilestone(si_->cloneState(workStates[s]));
213 for (
unsigned int i = 0 ; i < s ; ++i)
216 Vertex m = boost::add_vertex(g_);
217 stateProperty_[m] = si_->cloneState(workStates[i]);
218 totalConnectionAttemptsProperty_[m] = 1;
219 successfulConnectionAttemptsProperty_[m] = 0;
220 disjointSets_.make_set(m);
223 const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
224 const unsigned int id = maxEdgeID_++;
225 const Graph::edge_property_type properties(weight,
id);
226 boost::add_edge(v, m, properties, g_);
227 uniteComponents(v, m);
236 if (s > 0 || !sameComponent(v, last))
239 const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
240 const unsigned int id = maxEdgeID_++;
241 const Graph::edge_property_type properties(weight,
id);
242 boost::add_edge(v, last, properties, g_);
243 uniteComponents(v, last);
245 graphMutex_.unlock();
260 sampler_ = si_->allocValidStateSampler();
263 growRoadmap (ptc, workState);
264 si_->freeState(workState);
275 while (!found && ptc ==
false)
277 unsigned int attempts = 0;
280 found = sampler_->sample(workState);
286 addMilestone(si_->cloneState(workState));
294 while (!ptc && !addedSolution_)
301 goalM_.push_back(addMilestone(si_->cloneState(st)));
305 addedSolution_ = haveSolution(startM_, goalM_, solution);
308 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
316 bool sol_cost_set =
false;
317 foreach (Vertex start, starts)
319 foreach (Vertex goal, goals)
323 bool same_component = sameComponent(start, goal);
324 graphMutex_.unlock();
333 if (opt_->isSatisfied(pathCost))
338 else if (!sol_cost_set || opt_->isCostBetterThan(pathCost, sol_cost))
354 return addedSolution_;
364 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
370 startM_.push_back(addMilestone(si_->cloneState(st)));
372 if (startM_.size() == 0)
374 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
380 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region", getName().c_str());
387 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
389 goalM_.push_back(addMilestone(si_->cloneState(st)));
393 OMPL_ERROR(
"%s: Unable to find any valid goal states", getName().c_str());
398 unsigned int nrStartStates = boost::num_vertices(g_);
399 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), nrStartStates);
402 addedSolution_ =
false;
410 constructRoadmap(ptcOrSolutionFound);
415 OMPL_INFORM(
"%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
421 if (addedNewSolution())
423 pdef_->addSolutionPath (psol);
435 sampler_ = si_->allocValidStateSampler();
437 simpleSampler_ = si_->allocStateSampler();
440 si_->allocStates(xstates);
443 while (ptc() ==
false)
454 si_->freeStates(xstates);
459 boost::mutex::scoped_lock _(graphMutex_);
461 Vertex m = boost::add_vertex(g_);
462 stateProperty_[m] = state;
463 totalConnectionAttemptsProperty_[m] = 1;
464 successfulConnectionAttemptsProperty_[m] = 0;
467 disjointSets_.make_set(m);
472 const std::vector<Vertex>& neighbors = connectionStrategy_(m);
474 foreach (Vertex n, neighbors)
475 if (connectionFilter_(m, n))
477 totalConnectionAttemptsProperty_[m]++;
478 totalConnectionAttemptsProperty_[n]++;
479 if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
481 successfulConnectionAttemptsProperty_[m]++;
482 successfulConnectionAttemptsProperty_[n]++;
483 const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
484 const unsigned int id = maxEdgeID_++;
485 const Graph::edge_property_type properties(weight,
id);
486 boost::add_edge(m, n, properties, g_);
487 uniteComponents(n, m);
496 disjointSets_.union_set(m1, m2);
501 return boost::same_component(m1, m2, disjointSets_);
506 boost::mutex::scoped_lock _(graphMutex_);
507 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
512 boost::astar_search(g_, start,
514 boost::predecessor_map(prev).
516 isCostBetterThan, opt_.get(), _1, _2)).
518 combineCosts, opt_.get(), _1, _2)).
519 distance_inf(opt_->infiniteCost()).
520 distance_zero(opt_->identityCost()).
521 visitor(AStarGoalVisitor<Vertex>(goal)));
523 catch (AStarFoundGoal&)
527 if (prev[goal] == goal)
528 throw Exception(name_,
"Could not find solution path");
530 return constructGeometricPath(prev, start, goal);
536 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
537 p->
append(stateProperty_[pos]);
538 p->
append(stateProperty_[start]);
546 Planner::getPlannerData(data);
549 for (
size_t i = 0; i < startM_.size(); ++i)
552 for (
size_t i = 0; i < goalM_.size(); ++i)
556 foreach(
const Edge e, boost::edges(g_))
558 const Vertex v1 = boost::source(e, g_);
559 const Vertex v2 = boost::target(e, g_);
568 data.
tagState(stateProperty_[v1], const_cast<PRM*>(
this)->disjointSets_.find_set(v1));
569 data.
tagState(stateProperty_[v2], const_cast<PRM*>(
this)->disjointSets_.find_set(v2));
575 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
unsigned int milestoneCount(void) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true...
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
bool addedNewSolution(void) const
Returns the value of the addedSolution_ member.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
bool optimized_
True of the solution was optimized to meet the specified optimization criterion.
The planner failed to find a solution.
Representation of a solution to a planning problem.
GoalType recognizedGoal
The type of goal specification the planner can use.
void freeMemory(void)
Free all the memory allocated by the planner.
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
virtual void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
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...
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...
Abstract definition of goals.
static const double ROADMAP_BUILD_TIME
The time in seconds for a single roadmap building operation (dt)
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void growRoadmap(double growTime)
If the user desires, the roadmap can be improved for the given time (seconds). The solve() method wil...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. Grows a roadmap using constructRoadmap(). This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work for more time on an unsolved problem, for example. Start and goal states from the currently specified ProblemDefinition are cached. This means that between calls to solve(), input states are only added, not removed. When using PRM as a multi-query planner, the input states should be however cleared, without clearing the roadmap itself. This can be done using the clearQuery() function.
A container that supports probabilistic sampling over weighted data.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
Abstract definition of a goal region that can be sampled.
static const unsigned int DEFAULT_NEAREST_NEIGHBORS
The number of nearest neighbors to consider by default in the construction of the PRM roadmap...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
The goal is of a type that a planner does not recognize.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
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...
virtual Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
virtual 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...
The planner found an exact solution.
void reverse(void)
Reverse the path.
A class to store the exit status of Planner::solve()
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...
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...
virtual bool couldSample(void) const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
An optimization objective which corresponds to optimizing path length.
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...
Definition of an abstract state.
virtual unsigned int maxSampleCount(void) const =0
Return the maximum number of samples that can be asked for before repeating.
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 ...
Abstract definition of optimization objectives.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
The exception type for ompl.
The planner found an approximate solution.
void clearQuery(void)
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element, which can be used to subsequently update or remove the data from the PDF.
bool haveSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Make the minimal number of connections required to ensure asymptotic optimality.
void uniteComponents(Vertex m1, Vertex m2)
Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
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...
virtual base::PathPtr constructGeometricPath(const boost::vector_property_map< Vertex > &prev, const Vertex &start, const Vertex &goal)
Given a solution represented as a vector of predecesors in the roadmap, construct a geometric path...
Definition of a geometric path.
bool empty(void) const
Returns whether the PDF contains no data.
_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...
virtual void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
A boost shared pointer wrapper for ompl::base::Path.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
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...
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.