PRM.cpp
78 successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_)),
91 Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors, std::string("8:1000"));
119 connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension());
144 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
236 unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
263 // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
298 /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
322 base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
341 bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution)
349 // we lock because the connected components algorithm is incremental and may change disjointSets_
386 ompl::base::PlannerStatus ompl::geometric::PRM::solve(const base::PlannerTerminationCondition &ptc)
389 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
428 OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
435 // construct new planner termination condition that fires when the given ptc is true, or a solution is found
437 base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&PRM::addedNewSolution, this)));
444 OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
477 growRoadmap(base::plannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(2.0 * magic::ROADMAP_BUILD_TIME)), xstates[0]);
479 expandRoadmap(base::plannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(magic::ROADMAP_BUILD_TIME)), xstates);
532 ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex &start, const Vertex &goal)
573 data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], const_cast<PRM*>(this)->disjointSets_.find_set(startM_[i])));
576 data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], const_cast<PRM*>(this)->disjointSets_.find_set(goalM_[i])));
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:73
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
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:391
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true...
Definition: PlannerTerminationCondition.cpp:213
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: PRM.cpp:170
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:319
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
The planner failed to find a solution.
Definition: PlannerStatus.h:62
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:72
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:208
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:317
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition: PRM.cpp:198
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:527
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...
Definition: PlannerData.cpp:434
static const double ROADMAP_BUILD_TIME
The time in seconds for a single roadmap building operation (dt)
Definition: PRM.cpp:65
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
Definition: PRM.h:87
STL namespace.
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:278
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
Definition: PathGeometric.cpp:432
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PRM.cpp:177
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.
Definition: PRM.cpp:386
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
Definition: PlannerTerminationCondition.cpp:223
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition: PRM.h:127
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:49
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:69
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
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...
Definition: PlannerData.cpp:578
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:486
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:532
The planner found an exact solution.
Definition: PlannerStatus.h:66
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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...
Definition: PlannerData.cpp:444
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 boost shared pointer wrapper for ompl::base::SpaceInformation.
An optimization objective which corresponds to optimizing path length.
Definition: PathLengthOptimizationObjective.h:47
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: PlannerData.cpp:425
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 ...
Definition: MagicConstants.h:103
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:66
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...
Definition: ProblemDefinition.h:98
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.
Definition: PDF.h:97
virtual bool couldSample() 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.
Definition: GoalSampleableRegion.h:78
Make the minimal number of connections required to ensure asymptotic optimality.
Definition: ConnectionStrategy.h:127
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:522
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
Definition: PRM.cpp:149
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PRM.cpp:108
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: PRM.cpp:164
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:218
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:62
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:341
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
Definition: PRM.h:259
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: PRM.h:125
_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
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:458
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition: ProblemDefinition.h:106
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
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...
Definition: PRM.cpp:567
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:381
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:596
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:55