RRTstar.cpp
35 /* Authors: Alejandro Perez, Sertac Karaman, Ryan Luna, Luis G. Torres, Ioan Sucan, Javier V Gomez, Jonathan Gammell */
59 Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange, "0.:1.:10000.");
60 Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias, "0.:.05:1.");
61 Planner::declareParam<double>("rewire_factor", this, &RRTstar::setRewireFactor, &RRTstar::getRewireFactor,
63 Planner::declareParam<bool>("use_k_nearest", this, &RRTstar::setKNearest, &RRTstar::getKNearest, "0,1");
64 Planner::declareParam<bool>("delay_collision_checking", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC, "0,1");
65 Planner::declareParam<bool>("tree_pruning", this, &RRTstar::setTreePruning, &RRTstar::getTreePruning, "0,1");
66 Planner::declareParam<double>("prune_threshold", this, &RRTstar::setPruneThreshold, &RRTstar::getPruneThreshold,
68 Planner::declareParam<bool>("pruned_measure", this, &RRTstar::setPrunedMeasure, &RRTstar::getPrunedMeasure, "0,1");
69 Planner::declareParam<bool>("informed_sampling", this, &RRTstar::setInformedSampling, &RRTstar::getInformedSampling,
71 Planner::declareParam<bool>("sample_rejection", this, &RRTstar::setSampleRejection, &RRTstar::getSampleRejection,
77 Planner::declareParam<bool>("ordered_sampling", this, &RRTstar::setOrderedSampling, &RRTstar::getOrderedSampling,
79 Planner::declareParam<unsigned int>("ordering_batch_size", this, &RRTstar::setBatchSize, &RRTstar::getBatchSize,
81 Planner::declareParam<bool>("focus_search", this, &RRTstar::setFocusSearch, &RRTstar::getFocusSearch, "0,1");
82 Planner::declareParam<unsigned int>("number_sampling_attempts", this, &RRTstar::setNumSamplingAttempts,
99 if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
101 OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
106 nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
119 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
134 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
165 ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
203 OMPL_INFORM("%s: Started planning with %u states. Seeking a solution better than %.5f.", getName().c_str(), nn_->size(), opt_->getCostThreshold().value());
205 if ((useTreePruning_ || useRejectionSampling_ || useInformedSampling_ || useNewStateRejection_) &&
207 OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy "
212 const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
241 std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
252 // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
259 // Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
595 OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
599 // We've added a solution if newSolution == true, and it is an approximate solution if bestGoalMotion_ == false
615 maxDistance_, r_rrt_ * std::pow(log(cardDbl) / cardDbl, 1 / static_cast<double>(si_->getStateDimension())));
672 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
695 // We are only pruning motions if they, AND all descendents, have a estimated cost greater than pruneTreeCost
696 // The easiest way to do this is to find leaves that should be pruned and ascend up their ancestry until a
698 // To avoid making an intermediate copy of the NN structure, we process the tree by descending down from the
700 // In the first pass, all Motions with a cost below pruneTreeCost, or Motion's with children with costs below
702 // while all other Motions are stored as either a 'leaf' or 'chain' Motion. After all the leaves are
705 // This avoids (1) copying the NN structure into an intermediate variable and (2) the use of the expensive
807 goalMotions_.erase(std::remove(goalMotions_.begin(), goalMotions_.end(), leavesToPrune.front()),
875 void ompl::geometric::RRTstar::addChildrenToList(std::queue<Motion *, std::deque<Motion *>> *motionList, Motion *motion)
883 bool ompl::geometric::RRTstar::keepCondition(const Motion *motion, const base::Cost &threshold) const
886 // if !(threshold < heuristic), as if b is not better than a, then a is better than, or equal to, b
889 // If the threshold is the theoretical minimum, the bestGoalMotion_ will sometimes fail the test due to floating point precision. Avoid that.
918 opt_->costToGo(motion->state, pdef_->getGoal().get()); // lower-bounding cost from the state to the goal
928 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
932 // If we just disabled tree pruning, but we wee using prunedMeasure, we need to disable that as it required myself
948 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
955 OMPL_ERROR("%s: InformedMeasure requires InformedSampling and TreePruning.", getName().c_str());
991 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
998 OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
1001 // If we just disabled tree pruning, but we are using prunedMeasure, we need to disable that as it required myself
1007 // Check if we're changing the setting of informed sampling. If we are, we will need to create a new sampler, which
1039 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
1046 OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
1049 // Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which
1074 OMPL_ERROR("%s: OrderedSampling requires either informed sampling or rejection sampling.", getName().c_str());
1077 // Check if we're changing the setting. If we are, we will need to create a new sampler, which we only want to do if
1102 // We are using informed sampling, this can end-up reverting to rejection sampling in some cases
1152 k_rrt_ = rewireFactor_ * (std::pow(2, dimDbl + 1) * boost::math::constants::e<double>() * (1.0 + 1.0 / dimDbl));
1155 // If we're not using the informed measure, prunedMeasure_ will be set to si_->getSpaceMeasure();
1158 std::pow(2 * (1.0 + 1.0 / dimDbl) * (prunedMeasure_ / unitNBallMeasure(si_->getStateDimension())), 1.0 / dimDbl);
void setAdmissibleCostToCome(const bool admissible)
Controls whether pruning and new-state rejection uses an admissible cost-to-come estimate or not.
Definition: RRTstar.h:340
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:406
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
Definition: ProblemDefinition.h:153
int pruneTree(const base::Cost &pruneTreeCost)
Prunes all those states which estimated total cost is higher than pruneTreeCost. Returns the number o...
Definition: RRTstar.cpp:676
void removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
Definition: RRTstar.cpp:620
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTstar.cpp:1033
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:467
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
base::Cost incCost
The incremental cost of this motion's parent to this motion (this is stored to save distance computat...
Definition: RRTstar.h:454
void getNeighbors(Motion *motion, std::vector< Motion * > &nbh) const
Gets the neighbours of a given motion, using either k-nearest of radius as appropriate.
Definition: RRTstar.cpp:603
double getPruneThreshold() const
Get the current prune states percentage threshold parameter.
Definition: RRTstar.h:289
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:133
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:120
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
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: RRTstar.cpp:165
void setFocusSearch(const bool focus)
A meta parameter to focusing the search to improving the current solution. This is the parameter set ...
Definition: RRTstar.h:379
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Definition: GeometricEquations.cpp:55
void setBatchSize(unsigned int batchSize)
Set the batch size used for sample ordering.
Definition: RRTstar.h:362
void setTreePruning(bool prune)
Controls whether the tree is pruned during the search. This pruning removes a vertex if and only if i...
Definition: RRTstar.cpp:922
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTstar.cpp:145
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:127
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTstar.h:320
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1147
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: RRTstar.cpp:656
bool getDelayCC() const
Get the state of the delayed collision checking option.
Definition: RRTstar.h:260
double getRewireFactor() const
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg* > r_rrg* (or k_rrg = s \times k_r...
Definition: RRTstar.h:231
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
Definition: RRTstar.h:254
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:263
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTstar.cpp:94
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:112
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition: Planner.h:275
base::Cost solutionHeuristic(const Motion *motion) const
Computes the solution cost heuristically as the cost to come from start to the motion plus the cost t...
Definition: RRTstar.cpp:896
void setPrunedMeasure(bool informedMeasure)
Use the measure of the pruned subproblem instead of the measure of the entire problem domain (if such...
Definition: RRTstar.cpp:942
void updateChildCosts(Motion *m)
Updates the cost of the children of this node if the cost up to this node has changed.
Definition: RRTstar.cpp:632
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg* (or k_rrg = s \times k_rrg*)
Definition: RRTstar.h:223
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition: RRTstar.cpp:985
void setOrderedSampling(bool orderSamples)
Controls whether samples are returned in ordered by the heuristic. This is accomplished by generating...
Definition: RRTstar.cpp:1069
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTstar.h:400
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:412
bool getNewStateRejection() const
Get the state of the new-state rejection option.
Definition: RRTstar.h:333
void setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition: RRTstar.h:394
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:161
std::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
Definition: ProblemDefinition.h:209
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:413
bool getAdmissibleCostToCome() const
Get the admissibility of the pruning and new-state rejection heuristic.
Definition: RRTstar.h:346
void addChildrenToList(std::queue< Motion *, std::deque< Motion * >> *motionList, Motion *motion)
Add the children of a vertex to the given list.
Definition: RRTstar.cpp:875
void setPruneThreshold(const double pp)
Set the fractional change in solution cost necessary for pruning to occur, i.e., prune if the new sol...
Definition: RRTstar.h:283
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
Definition: PlannerTerminationCondition.cpp:172
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition: ProblemDefinition.h:169
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:432
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:422
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:111
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTstar.h:210
void setNewStateRejection(const bool reject)
Controls whether heuristic rejection is used on new states before connection (e.g....
Definition: RRTstar.h:327
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:457
bool keepCondition(const Motion *motion, const base::Cost &threshold) const
Check whether the given motion passes the specified cost threshold, meaning it will be kept during pr...
Definition: RRTstar.cpp:883
unsigned int getBatchSize() const
Get the batch size used for sample ordering.
Definition: RRTstar.h:368