LazyPRM.cpp
87 Planner::declareParam<double>("range", this, &LazyPRM::setRange, &LazyPRM::getRange, "0.:1.:10000.");
88 Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &LazyPRM::setMaxNearestNeighbors, std::string("8:1000"));
118 connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&LazyPRM::milestoneCount, this), nn_, si_->getStateDimension());
120 connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
143 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
233 ompl::base::PlannerStatus ompl::geometric::LazyPRM::solve(const base::PlannerTerminationCondition &ptc)
236 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
275 OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
297 if (solComponent != -1 && (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent))
349 OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
389 long int ompl::geometric::LazyPRM::solutionComponent(std::pair<std::size_t, std::size_t> *startGoalPair) const
407 ompl::base::PathPtr ompl::geometric::LazyPRM::constructSolution(const Vertex &start, const Vertex &goal)
464 for (std::set<Vertex>::iterator it = milestonesToRemove.begin() ; it != milestonesToRemove.end() ; ++it)
496 std::vector<const base::State*>::const_iterator prevState = states.begin(), state = prevState + 1;
523 for (std::vector<const base::State*>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
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
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:64
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: LazyPRM.h:294
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: LazyPRM.h:327
LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: LazyPRM.cpp:68
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition: LazyPRM.h:349
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
boost::property_map< Graph, boost::vertex_index_t >::type indexProperty_
Access to the internal base::state at each Vertex.
Definition: LazyPRM.h:324
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or NULL if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:271
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
boost::property_map< Graph, vertex_component_t >::type vertexComponentProperty_
Access the connected component of a vertex.
Definition: LazyPRM.h:333
ConnectionStrategy connectionStrategy_
Function that returns the milestones to attempt connections with.
Definition: LazyPRM.h:297
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
STL namespace.
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 unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
static const unsigned int VALIDITY_UNKNOWN
Flag indicating validity of an edge of a vertex.
Definition: LazyPRM.h:241
ConnectionFilter connectionFilter_
Function that can reject a milestone connection.
Definition: LazyPRM.h:300
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
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:49
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: LazyPRM.cpp:233
Return at most k neighbors, as long as they are also within a specified bound.
Definition: ConnectionStrategy.h:167
boost::property_map< Graph, vertex_flags_t >::type vertexValidityProperty_
Access the validity state of a vertex.
Definition: LazyPRM.h:336
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
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:207
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
Definition: LazyPRM.cpp:159
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
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LazyPRM.cpp:187
The planner found an exact solution.
Definition: PlannerStatus.h:66
boost::adjacency_list_traits< boost::vecS, boost::listS, boost::undirectedS >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: LazyPRM.h:96
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:407
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:343
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
A boost shared pointer wrapper for ompl::base::SpaceInformation.
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: LazyPRM.cpp:180
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
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
Definition: LazyPRM.h:213
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:66
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
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:284
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
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. If so, return the id of that component. Otherwise, return -1.
Definition: LazyPRM.cpp:389
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:528
void setRange(double distance)
Set the maximum length of a motion to be added to the roadmap.
Definition: LazyPRM.cpp:150
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: LazyPRM.cpp:533
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
static const unsigned int VALIDITY_TRUE
Flag indicating validity of an edge of a vertex.
Definition: LazyPRM.h:244
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:232
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition: LazyPRM.h:127
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:218
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:170
bool userSetConnectionStrategy_
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
Definition: LazyPRM.h:303
boost::property_map< Graph, edge_flags_t >::type edgeValidityProperty_
Access the validity state of an edge.
Definition: LazyPRM.h:339
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:59
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
std::map< unsigned long int, unsigned long int > componentSize_
The number of elements in each component in the LazyPRM roadmap.
Definition: LazyPRM.h:346
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LazyPRM.cpp:104
A boost shared pointer wrapper for ompl::base::Path.
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: LazyPRM.cpp:174
base::StateSamplerPtr sampler_
Sampler user for generating random in the state space.
Definition: LazyPRM.h:309
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:55