RRTXstatic.cpp
59 Planner::declareParam<double>("range", this, &RRTXstatic::setRange, &RRTXstatic::getRange, "0.:1.:10000.");
60 Planner::declareParam<double>("goal_bias", this, &RRTXstatic::setGoalBias, &RRTXstatic::getGoalBias, "0.:.05:1.");
61 Planner::declareParam<double>("epsilon", this, &RRTXstatic::setEpsilon, &RRTXstatic::getEpsilon, "0.:.01:10.");
62 Planner::declareParam<double>("rewire_factor", this, &RRTXstatic::setRewireFactor, &RRTXstatic::getRewireFactor,
65 Planner::declareParam<bool>("use_k_nearest", this, &RRTXstatic::setKNearest, &RRTXstatic::getKNearest, "0,1");
66 Planner::declareParam<bool>("update_children", this, &RRTXstatic::setUpdateChildren, &RRTXstatic::getUpdateChildren,
68 Planner::declareParam<int>("rejection_variant", this, &RRTXstatic::setVariant, &RRTXstatic::getVariant, "0:3");
69 Planner::declareParam<double>("rejection_variant_alpha", this, &RRTXstatic::setAlpha, &RRTXstatic::getAlpha, "0.:"
76 Planner::declareParam<unsigned int>("number_sampling_attempts", this, &RRTXstatic::setNumSamplingAttempts,
94 if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
96 OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
101 nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
114 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
129 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
154 ompl::base::PlannerStatus ompl::geometric::RRTXstatic::solve(const base::PlannerTerminationCondition &ptc)
189 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
192 OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy "
197 const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
232 std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
243 // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
250 // Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
357 // Remove element from the queue and NULL the handle so that we know it's not in the queue anymore
552 OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
561 // If x->handle is not NULL, x is already in the queue and needs to be update, otherwise it is inserted
620 return opt_->isCostBetterThan(mc_.alphaCostPlusHeuristic(x, alpha_), opt_->infiniteCost()); // Always true?
661 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
671 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
678 OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
681 // Check if we're changing the setting of informed sampling. If we are, we will need to create a new sampler, which
707 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
714 OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
717 // Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which
742 // We are using informed sampling, this can end-up reverting to rejection sampling in some cases
785 k_rrt_ = rewireFactor_ * (std::pow(2, dimDbl + 1) * boost::math::constants::e<double>() * (1.0 + 1.0 / dimDbl));
789 std::pow(2 * (1.0 + 1.0 / dimDbl) * (si_->getSpaceMeasure() / unitNBallMeasure(si_->getStateDimension())),
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void setVariant(const int variant)
Set variant used for rejection sampling.
Definition: RRTXstatic.h:347
double getEpsilon() const
Get the threshold epsilon the planner is using.
Definition: RRTXstatic.h:275
std::vector< std::pair< Motion *, bool > > nbh
The set of neighbors of this motion with a boolean indicating if the feasibility of edge as been test...
Definition: RRTXstatic.h:447
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
Definition: ProblemDefinition.h:153
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTXstatic.h:285
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTXstatic.cpp:89
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
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTXstatic.h:329
bool getInformedSampling() const
Get the state direct heuristic sampling.
Definition: RRTXstatic.h:239
bool includeVertex(const Motion *x) const
Test if the vertex should be included according to the variant in use.
Definition: RRTXstatic.cpp:615
void setAlpha(const double a)
Set the value alpha used for rejection sampling.
Definition: RRTXstatic.h:361
void getNeighbors(Motion *motion) const
Gets the neighbours of a given motion, using either k-nearest of radius as appropriate.
Definition: RRTXstatic.cpp:592
void setUpdateChildren(bool val)
Set whether or not to always propagate cost updates to children.
Definition: RRTXstatic.h:335
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
Representation of a motion (node of the tree)
Definition: RRTXstatic.h:422
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTXstatic.cpp:780
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: RRTXstatic.h:298
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 setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTXstatic.cpp:701
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:127
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTXstatic.h:254
void setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition: RRTXstatic.h:323
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTXstatic.h:248
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:263
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
void removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
Definition: RRTXstatic.cpp:572
BinaryHeap< Motion *, MotionCompare >::Element * handle
Handle to identify the motion in the queue.
Definition: RRTXstatic.h:450
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition: RRTXstatic.cpp:665
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: RRTXstatic.cpp:645
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition: RRTXstatic.h:260
Defines the operator to compare motions.
Definition: RRTXstatic.h:386
bool getUpdateChildren() const
True if the cost is always propagate to children.
Definition: RRTXstatic.h:341
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: RRTXstatic.h:306
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTXstatic.cpp:137
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
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: RRTXstatic.cpp:154
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: RRTXstatic.h:443