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