void ompl::geometric::FMT::assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
{
// Ensure that there is at least one node near each goal
while (const base::State *goalState = pis_.nextGoal())
{
Motion *gMotion = new Motion(si_);
si_->copyState(gMotion->getState(), goalState);
std::vector<Motion*> nearGoal;
nn_->nearestR(gMotion, goal->getThreshold(), nearGoal);
// If there is no node in the goal region, insert one
if (nearGoal.empty())
{
OMPL_DEBUG("No state inside goal region");
if (si_->getStateValidityChecker()->isValid(gMotion->getState()))
{
nn_->add(gMotion);
goalState_ = gMotion->getState();
}
else
{
si_->freeState(gMotion->getState());
delete gMotion;
}
}
else // There is already a sample in the goal region
{
goalState_ = nearGoal[0]->getState();
si_->freeState(gMotion->getState());
delete gMotion;
}
} // For each goal
}
void ompl::geometric::FMT::sampleFree(const base::PlannerTerminationCondition &ptc)
{
unsigned int nodeCount = 0;
unsigned int sampleAttempts = 0;
Motion *motion = new Motion(si_);
// Sample numSamples_ number of nodes from the free configuration space
while (nodeCount < numSamples_ && !ptc)
{
sampler_->sampleUniform(motion->getState());
sampleAttempts++;
bool collision_free = si_->isValid(motion->getState());
if (collision_free)
{
nodeCount++;
nn_->add(motion);
motion = new Motion(si_);
} // If collision free
} // While nodeCount < numSamples
si_->freeState(motion->getState());
delete motion;
// 95% confidence limit for an upper bound for the true free space volume
freeSpaceVolume_ = boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) * si_->getStateSpace()->getMeasure();
}
bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
{
// Find all nodes that are near z, and also in set Unvisited
std::vector<Motion*> xNear;
const std::vector<Motion*> &zNeighborhood = neighborhoods_[*z];
const unsigned int zNeighborhoodSize = zNeighborhood.size();
xNear.reserve(zNeighborhoodSize);
for (unsigned int i = 0; i < zNeighborhoodSize; ++i)
{
Motion *x = zNeighborhood[i];
if (x->getSetType() == Motion::SET_UNVISITED)
{
saveNeighborhood(x);
if (nearestK_)
{
// Only include neighbors that are mutually k-nearest
// Relies on NN datastructure returning k-nearest in sorted order
const base::Cost connCost = opt_->motionCost((*z)->getState(), x->getState());
const base::Cost worstCost = opt_->motionCost(neighborhoods_[x].back()->getState(), x->getState());
if (opt_->isCostBetterThan(worstCost, connCost))
continue;
else
xNear.push_back(x);
}
else
xNear.push_back(x);
}
}
// For each node near z and in set Unvisited, attempt to connect it to set Open
std::vector<Motion*> yNear;
std::vector<Motion*> Open_new;
const unsigned int xNearSize = xNear.size();
for (unsigned int i = 0 ; i < xNearSize; ++i)
{
Motion *x = xNear[i];
// Find all nodes that are near x and in set Open
const std::vector<Motion*> &xNeighborhood = neighborhoods_[x];
const unsigned int xNeighborhoodSize = xNeighborhood.size();
yNear.reserve(xNeighborhoodSize);
for (unsigned int j = 0; j < xNeighborhoodSize; ++j)
{
if (xNeighborhood[j]->getSetType() == Motion::SET_OPEN)
yNear.push_back(xNeighborhood[j]);
}
// Find the lowest cost-to-come connection from Open to x
base::Cost cMin(std::numeric_limits<double>::infinity());
Motion *yMin = getBestParent(x, yNear, cMin);
yNear.clear();
// If an optimal connection from Open to x was found
if (yMin != nullptr)
{
bool collision_free = false;
if (cacheCC_)
{
if (!yMin->alreadyCC(x))
{
collision_free = si_->checkMotion(yMin->getState(), x->getState());
++collisionChecks_;
// Due to FMT* design, it is only necessary to save unsuccesful
// connection attemps because of collision
if (!collision_free)
yMin->addCC(x);
}
}
else
{
++collisionChecks_;
collision_free = si_->checkMotion(yMin->getState(), x->getState());
}
if (collision_free)
{
// Add edge from yMin to x
x->setParent(yMin);
x->setCost(cMin);
x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), goalState_));
yMin->getChildren().push_back(x);
// Add x to Open
Open_new.push_back(x);
// Remove x from Unvisited
x->setSetType(Motion::SET_CLOSED);
}
} // An optimal connection from Open to x was found
} // For each node near z and in set Unvisited, try to connect it to set Open
// Update Open
Open_.pop();
(*z)->setSetType(Motion::SET_CLOSED);
// Add the nodes in Open_new to Open
unsigned int openNewSize = Open_new.size();
for (unsigned int i = 0; i < openNewSize; ++i)
{
Open_.insert(Open_new[i]);
Open_new[i]->setSetType(Motion::SET_OPEN);
}
Open_new.clear();
if (Open_.empty())
{
if(!extendedFMT_)
OMPL_INFORM("Open is empty before path was found --> no feasible path exists");
return false;
}
// Take the top of Open as the new z
*z = Open_.top()->data;
return true;
}
ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTerminationCondition &ptc)
{
if (lastGoalMotion_) {
OMPL_INFORM("solve() called before clear(); returning previous solution");
traceSolutionPathThroughTree(lastGoalMotion_);
OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
return base::PlannerStatus(true, false);
}
else if (Open_.size() > 0)
{
OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
clear();
}
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
Motion *initMotion = nullptr;
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
// Add start states to V (nn_) and Open
while (const base::State *st = pis_.nextStart())
{
initMotion = new Motion(si_);
si_->copyState(initMotion->getState(), st);
Open_.insert(initMotion);
initMotion->setSetType(Motion::SET_OPEN);
initMotion->setCost(opt_->initialCost(initMotion->getState()));
nn_->add(initMotion); // V <-- {x_init}
}
if (!initMotion)
{
OMPL_ERROR("Start state undefined");
return base::PlannerStatus::INVALID_START;
}
// Sample N free states in the configuration space
if (!sampler_)
sampler_ = si_->allocStateSampler();
sampleFree(ptc);
assureGoalIsSampled(goal);
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
// Calculate the nearest neighbor search radius
/// \todo Create a PRM-like connection strategy
if (nearestK_)
{
NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
(boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
log((double)nn_->size()));
OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
}
else
{
NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
OMPL_DEBUG("Using radius of %f", NNr_);
}
// Execute the planner, and return early if the planner returns a failure
bool plannerSuccess = false;
bool successfulExpansion = false;
Motion *z = initMotion; // z <-- xinit
saveNeighborhood(z);
while (!ptc)
{
if ((plannerSuccess = goal->isSatisfied(z->getState())))
break;
successfulExpansion = expandTreeFromNode(&z);
if (!extendedFMT_ && !successfulExpansion)
break;
else if (extendedFMT_ && !successfulExpansion)
{
//Apply RRT*-like connections: sample and connect samples to tree
std::vector<Motion*> nbh;
std::vector<base::Cost> costs;
std::vector<base::Cost> incCosts;
std::vector<std::size_t> sortedCostIndices;
// our functor for sorting nearest neighbors
CostIndexCompare compareFn(costs, *opt_);
Motion *m = new Motion(si_);
while (!ptc && Open_.empty())
{
sampler_->sampleUniform(m->getState());
if (!si_->isValid(m->getState()))
continue;
if (nearestK_)
nn_->nearestK(m, NNk_, nbh);
else
nn_->nearestR(m, NNr_, nbh);
// Get neighbours in the tree.
std::vector<Motion*> yNear;
yNear.reserve(nbh.size());
for (std::size_t j = 0; j < nbh.size(); ++j)
{
if (nbh[j]->getSetType() == Motion::SET_CLOSED)
{
if (nearestK_)
{
// Only include neighbors that are mutually k-nearest
// Relies on NN datastructure returning k-nearest in sorted order
const base::Cost connCost = opt_->motionCost(nbh[j]->getState(), m->getState());
const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[j]].back()->getState(), nbh[j]->getState());
if (opt_->isCostBetterThan(worstCost, connCost))
continue;
else
yNear.push_back(nbh[j]);
}
else
yNear.push_back(nbh[j]);
}
}
// Sample again if the new sample does not connect to the tree.
if (yNear.empty())
continue;
// cache for distance computations
//
// Our cost caches only increase in size, so they're only
// resized if they can't fit the current neighborhood
if (costs.size() < yNear.size())
{
costs.resize(yNear.size());
incCosts.resize(yNear.size());
sortedCostIndices.resize(yNear.size());
}
// Finding the nearest neighbor to connect to
// By default, neighborhood states are sorted by cost, and collision checking
// is performed in increasing order of cost
//
// calculate all costs and distances
for (std::size_t i = 0 ; i < yNear.size(); ++i)
{
incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
}
// sort the nodes
//
// we're using index-value pairs so that we can get at
// original, unsorted indices
for (std::size_t i = 0; i < yNear.size(); ++i)
sortedCostIndices[i] = i;
std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(),
compareFn);
// collision check until a valid motion is found
for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
i != sortedCostIndices.begin() + yNear.size();
++i)
{
if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
{
m->setParent(yNear[*i]);
yNear[*i]->getChildren().push_back(m);
const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
m->setSetType(Motion::SET_OPEN);
nn_->add(m);
saveNeighborhood(m);
updateNeighborhood(m,nbh);
Open_.insert(m);
z = m;
break;
}
}
} // while (!ptc && Open_.empty())
} // else if (extendedFMT_ && !successfulExpansion)
} // While not at goal
if (plannerSuccess)
{
// Return the path to z, since by definition of planner success, z is in the goal region
lastGoalMotion_ = z;
traceSolutionPathThroughTree(lastGoalMotion_);
OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
return base::PlannerStatus(true, false);
} // if plannerSuccess
else
{
// Planner terminated without accomplishing goal
return base::PlannerStatus(false, false);
}
}
