LBTRRT.cpp
59 Planner::declareParam<double>("range", this, &LBTRRT::setRange, &LBTRRT::getRange, "0.:1.:10000.");
60 Planner::declareParam<double>("goal_bias", this, &LBTRRT::setGoalBias, &LBTRRT::getGoalBias, "0.:.05:1.");
61 Planner::declareParam<double>("epsilon", this, &LBTRRT::setApproximationFactor, &LBTRRT::getApproximationFactor, "0.:.1:10.");
110 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str());
117 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
137 ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerminationCondition &ptc)
160 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
181 // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal states.
182 if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && goal_s->canSample())
316 OMPL_INFORM("%s: Created %u states. %u goal states in tree.", getName().c_str(), statesGenerated, goalMotions_.size());
330 if (opt_->isCostBetterThan(base::Cost((1.0 + epsilon_) * potentialLb.value()), child->costApx_))
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
void removeFromParentLb(Motion *m)
remove motion from its parent in the lower bound tree
Definition: LBTRRT.cpp:405
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
Definition: ProblemDefinition.h:91
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:104
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: LBTRRT.cpp:137
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:72
bool attemptNodeUpdate(Motion *potentialParent, Motion *child)
attempt to rewire the trees
Definition: LBTRRT.cpp:321
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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
STL namespace.
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: LBTRRT.cpp:367
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.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: LBTRRT.h:235
void removeFromParentApx(Motion *m)
remove motion from its parent in the approximation tree
Definition: LBTRRT.cpp:409
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: LBTRRT.h:251
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 void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LBTRRT.cpp:89
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: LBTRRT.h:266
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
void updateChildCostsLb(Motion *m)
update the child cost of the lower bound tree
Definition: LBTRRT.cpp:388
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Definition: GoalSampleableRegion.h:70
A boost shared pointer wrapper for ompl::base::SpaceInformation.
An optimization objective which corresponds to optimizing path length.
Definition: PathLengthOptimizationObjective.h:47
void updateChildCostsApx(Motion *m)
update the child cost of the approximation tree
Definition: LBTRRT.cpp:396
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
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: LBTRRT.h:108
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
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
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
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBTRRT.cpp:75
static const double kRRG
kRRG = 2e~5.5 is a valid choice for all problem instances
Definition: LBTRRT.h:154
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::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: LBTRRT.h:248
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition: LBTRRT.h:269
base::Cost incCost_
The incremental lower bound cost of this motion's parent to this motion (this is stored to save dista...
Definition: LBTRRT.h:191
Definition: LBTRRT.h:197
void removeFromParent(const Motion *m, std::vector< Motion * > &vec)
remove motion from a vector
Definition: LBTRRT.cpp:413
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
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.