void ompl::geometric::RRTstar::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
{
OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
}
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
nn_->setDistanceFunction(std::bind(&RRTstar::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
opt_.reset(new base::PathLengthOptimizationObjective(si_));
// Store the new objective in the problem def'n
pdef_->setOptimizationObjective(opt_);
}
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
// Get the measure of the entire space:
prunedMeasure_ = si_->getSpaceMeasure();
// Calculate some constants:
calculateRewiringLowerBounds();
// Set the bestCost_ and prunedCost_ as infinite
bestCost_ = opt_->infiniteCost();
prunedCost_ = opt_->infiniteCost();
}
void ompl::geometric::RRTXstatic::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
{
OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
}
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
"planning time.",
getName().c_str());
opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
// Store the new objective in the problem def'n
pdef_->setOptimizationObjective(opt_);
}
mc_ = MotionCompare(opt_, pdef_);
q_ = BinaryHeap<Motion *, MotionCompare>(mc_);
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
// Calculate some constants:
calculateRewiringLowerBounds();
// Set the bestCost_ and prunedCost_ as infinite
bestCost_ = opt_->infiniteCost();
}
void ompl::geometric::RRTstar::setPrunedMeasure(bool informedMeasure)
{
if (static_cast<bool>(opt_) == true)
{
if (opt_->hasCostToGoHeuristic() == false)
{
OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
}
}
// This option only works with informed sampling
if (informedMeasure == true && (useInformedSampling_ == false || useTreePruning_ == false))
{
OMPL_ERROR("%s: InformedMeasure requires InformedSampling and TreePruning.", getName().c_str());
}
// Check if we're changed and update parameters if we have:
if (informedMeasure != usePrunedMeasure_)
{
// Store the setting
usePrunedMeasure_ = informedMeasure;
// Update the prunedMeasure_ appropriately, if it has been configured.
if (setup_ == true)
{
if (usePrunedMeasure_)
{
prunedMeasure_ = infSampler_->getInformedMeasure(prunedCost_);
}
else
{
prunedMeasure_ = si_->getSpaceMeasure();
}
}
// And either way, update the rewiring radius if necessary
if (useKNearest_ == false)
{
calculateRewiringLowerBounds();
}
}
}
int ompl::geometric::RRTstar::pruneTree(const base::Cost& pruneTreeCost)
{
// Variable
// The percent improvement (expressed as a [0,1] fraction) in cost
double fracBetter;
// The number pruned
int numPruned = 0;
if (opt_->isFinite(prunedCost_))
{
fracBetter = std::abs((pruneTreeCost.value() - prunedCost_.value())/prunedCost_.value());
}
else
{
fracBetter = 1.0;
}
if (fracBetter > pruneThreshold_)
{
// We are only pruning motions if they, AND all descendents, have a estimated cost greater than pruneTreeCost
// The easiest way to do this is to find leaves that should be pruned and ascend up their ancestry until a motion is found that is kept.
// To avoid making an intermediate copy of the NN structure, we process the tree by descending down from the start(s).
// In the first pass, all Motions with a cost below pruneTreeCost, or Motion's with children with costs below pruneTreeCost are added to the replacement NN structure,
// while all other Motions are stored as either a 'leaf' or 'chain' Motion. After all the leaves are disconnected and deleted, we check
// if any of the the chain Motions are now leaves, and repeat that process until done.
// This avoids (1) copying the NN structure into an intermediate variable and (2) the use of the expensive NN::remove() method.
// Variable
// The queue of Motions to process:
std::queue<Motion*, std::deque<Motion*> > motionQueue;
// The list of leaves to prune
std::queue<Motion*, std::deque<Motion*> > leavesToPrune;
// The list of chain vertices to recheck after pruning
std::list<Motion*> chainsToRecheck;
//Clear the NN structure:
nn_->clear();
// Put all the starts into the NN structure and their children into the queue:
// We do this so that start states are never pruned.
for (auto & startMotion : startMotions_)
{
// Add to the NN
nn_->add(startMotion);
// Add their children to the queue:
addChildrenToList(&motionQueue, startMotion);
}
while (motionQueue.empty() == false)
{
// Test, can the current motion ever provide a better solution?
if (keepCondition(motionQueue.front(), pruneTreeCost))
{
// Yes it can, so it definitely won't be pruned
// Add it back into the NN structure
nn_->add(motionQueue.front());
//Add it's children to the queue
addChildrenToList(&motionQueue, motionQueue.front());
}
else
{
// No it can't, but does it have children?
if (motionQueue.front()->children.empty() == false)
{
// Yes it does.
// We can minimize the number of intermediate chain motions if we check their children
// If any of them won't be pruned, then this motion won't either. This intuitively seems
// like a nice balance between following the descendents forever.
// Variable
// Whether the children are definitely to be kept.
bool keepAChild = false;
// Find if any child is definitely not being pruned.
for (unsigned int i = 0u; keepAChild == false && i < motionQueue.front()->children.size(); ++i)
{
// Test if the child can ever provide a better solution
keepAChild = keepCondition(motionQueue.front()->children.at(i), pruneTreeCost);
}
// Are we *definitely* keeping any of the children?
if (keepAChild)
{
// Yes, we are, so we are not pruning this motion
// Add it back into the NN structure.
nn_->add(motionQueue.front());
}
else
{
// No, we aren't. This doesn't mean we won't though
// Move this Motion to the temporary list
chainsToRecheck.push_back(motionQueue.front());
}
// Either way. add it's children to the queue
addChildrenToList(&motionQueue, motionQueue.front());
}
else
{
// No, so we will be pruning this motion:
leavesToPrune.push(motionQueue.front());
}
}
// Pop the iterator, std::list::erase returns the next iterator
motionQueue.pop();
}
// We now have a list of Motions to definitely remove, and a list of Motions to recheck
// Iteratively check the two lists until there is nothing to to remove
while (leavesToPrune.empty() == false)
{
// First empty the leave-to-prune
while (leavesToPrune.empty() == false)
{
// Remove the leaf from its parent
removeFromParent(leavesToPrune.front());
// Erase the actual motion
// First free the state
si_->freeState(leavesToPrune.front()->state);
// then delete the pointer
delete leavesToPrune.front();
// And finally remove it from the list, erase returns the next iterator
leavesToPrune.pop();
// Update our counter
++numPruned;
}
// Now, we need to go through the list of chain vertices and see if any are now leaves
auto mIter = chainsToRecheck.begin();
while (mIter != chainsToRecheck.end())
{
// Is the Motion a leaf?
if ((*mIter)->children.empty() == true)
{
// It is, add to the removal queue
leavesToPrune.push(*mIter);
// Remove from this queue, getting the next
mIter = chainsToRecheck.erase(mIter);
}
else
{
// Is isn't, skip to the next
++mIter;
}
}
}
// Now finally add back any vertices left in chainsToReheck.
// These are chain vertices that have descendents that we want to keep
for (std::list<Motion*>::const_iterator mIter = chainsToRecheck.begin(); mIter != chainsToRecheck.end(); ++mIter)
{
// Add the motion back to the NN struct:
nn_->add(*mIter);
}
// All done pruning.
// Update the cost at which we've pruned:
prunedCost_ = pruneTreeCost;
// And if we're using the pruned measure, the measure to which we've pruned
if (usePrunedMeasure_)
{
prunedMeasure_ = infSampler_->getInformedMeasure(prunedCost_);
if (useKNearest_ == false)
{
calculateRewiringLowerBounds();
}
}
//No else, prunedMeasure_ is the si_ measure by default.
}
return numPruned;
}
