Loading...
Searching...
No Matches
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());
154ompl::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 "
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());
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());
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())),
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition BinaryHeap.h:53
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
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
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:172
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:106
Representation of a motion (node of the tree)
Definition RRTXstatic.h:327
BinaryHeap< Motion *, MotionCompare >::Element * handle
Handle to identify the motion in the queue.
Definition RRTXstatic.h:354
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
bool updateChildren_
Whether or not to propagate the cost to children if the update is less than epsilon.
Definition RRTXstatic.h:446
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
bool includeVertex(const Motion *x) const
Test if the vertex should be included according to the variant in use.
Definition RRTXstatic.cpp:615
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition RRTXstatic.h:397
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition RRTXstatic.h:425
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:572
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition RRTXstatic.cpp:701
unsigned int numSampleAttempts_
The number of attempts to make at informed sampling.
Definition RRTXstatic.h:467
void getNeighbors(Motion *motion) const
Gets the neighbours of a given motion, using either k-nearest of radius as appropriate.
Definition RRTXstatic.cpp:592
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition RRTXstatic.h:401
BinaryHeap< Motion *, MotionCompare > q_
Queue to order the nodes to update.
Definition RRTXstatic.h:440
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition RRTXstatic.cpp:137
double rewireFactor_
The rewiring factor, s, so that r_rrg = s \times r_rrg* > r_rrg* (or k_rrg = s \times k_rrg* > k_rrg*...
Definition RRTXstatic.h:414
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
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition RRTXstatic.h:367
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition RRTXstatic.cpp:780
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition RRTXstatic.cpp:665
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition RRTXstatic.h:428
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
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:144
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