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; }