bool ompl::geometric::TRRT::transitionTest(const base::Cost &motionCost)
{
// Disallow any cost that is not better than the cost threshold
if (!opt_->isCostBetterThan(motionCost, costThreshold_))
return false;
// Always accept if the cost is near or below zero
if (motionCost.value() < 1e-4)
return true;
double dCost = motionCost.value();
double transitionProbability = exp(-dCost / temp_);
if (transitionProbability > 0.5)
{
double costRange = worstCost_.value() - bestCost_.value();
if (fabs(costRange) > 1e-4) // Do not divide by zero
// Successful transition test. Decrease the temperature slightly
temp_ /= exp(dCost / (0.1 * costRange));
return true;
}
// The transition failed. Increase the temperature (slightly)
temp_ *= tempChangeFactor_;
return false;
}
bool ompl::geometric::TRRTConnect::transitionTest(base::Cost cost, double distance,
TreeData &tree, bool updateTemp) {
//Difference in cost
double slope(cost.value() / std::min(distance,maxDistance_));
//The probability of acceptance of a new motion is defined by its cost.
//Based on the Metropolis criterion.
double transitionProbability(exp(-slope/(kConstant_*tree.temp_)));
//Check if we can accept it
if (rng_.uniform01() <= transitionProbability) {//State has succeed
if (updateTemp) {
++tree.numStatesSucceed_;
//Update temperature
if (tree.numStatesSucceed_ > maxStatesSucceed_) {
tree.temp_ /= tempChangeFactor_;
//Prevent temperature from getting too small
if (tree.temp_ < minTemperature_) tree.temp_ = minTemperature_;
tree.numStatesSucceed_ = 0;
}
}
return true;
} else {//State has failed
if (updateTemp) {
++tree.numStatesFailed_;
//Update temperature
if (tree.numStatesFailed_ > maxStatesFailed_) {
tree.temp_ *= tempChangeFactor_;
tree.numStatesFailed_ = 0;
}
}
return false;
}
}
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;
}