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 "
127 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
155 ompl::base::PlannerStatus ompl::geometric::RRTXstatic::solve(const base::PlannerTerminationCondition &ptc)
190 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
193 OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy "
198 const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
233 std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
244 // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
251 // Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
358 // Remove element from the queue and NULL the handle so that we know it's not in the queue anymore
553 OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
562 // If x->handle is not NULL, x is already in the queue and needs to be update, otherwise it is inserted
621 return opt_->isCostBetterThan(mc_.alphaCostPlusHeuristic(x, alpha_), opt_->infiniteCost()); // Always true?
662 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
672 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
679 OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
682 // Check if we're changing the setting of informed sampling. If we are, we will need to create a new sampler, which
708 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
715 OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
718 // Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which
743 // We are using informed sampling, this can end-up reverting to rejection sampling in some cases
786 k_rrt_ = rewireFactor_ * (std::pow(2, dimDbl + 1) * boost::math::constants::e<double>() * (1.0 + 1.0 / dimDbl));
790 std::pow(2 * (1.0 + 1.0 / dimDbl) * (si_->getSpaceMeasure() / unitNBallMeasure(si_->getStateDimension())),
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:48
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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
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
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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
Definition: PlannerTerminationCondition.cpp:171
Representation of a motion (node of the tree)
Definition: RRTXstatic.h:327
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTXstatic.h:347
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:351
BinaryHeap< Motion *, MotionCompare >::Element * handle
Handle to identify the motion in the queue.
Definition: RRTXstatic.h:354
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:646
bool includeVertex(const Motion *x) const
Test if the vertex should be included according to the variant in use.
Definition: RRTXstatic.cpp:616
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 removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
Definition: RRTXstatic.cpp:573
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTXstatic.cpp:702
void getNeighbors(Motion *motion) const
Gets the neighbours of a given motion, using either k-nearest of radius as appropriate.
Definition: RRTXstatic.cpp:593
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTXstatic.cpp:138
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:155
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTXstatic.cpp:781
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition: RRTXstatic.cpp:666
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
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:145
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
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Definition: GeometricEquations.cpp:55
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:70
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
Definition: ProblemDefinition.h:89
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition: ProblemDefinition.h:105
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:97
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
Defines the operator to compare motions.
Definition: RRTXstatic.h:291