ompl/geometric/planners/fmt/src/FMT.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Autonomous Systems Laboratory, Stanford University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Stanford University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Authors: Ashley Clark (Stanford) and Wolfgang Pointner (AIT) */ 00036 /* Co-developers: Brice Rebsamen (Stanford) and Tim Wheeler (Stanford) */ 00037 /* Algorithm design: Lucas Janson (Stanford) and Marco Pavone (Stanford) */ 00038 /* Acknowledgements for insightful comments: Edward Schmerling (Stanford), Oren Salzman (Tel Aviv University), and Joseph Starek (Stanford) */ 00039 00040 #include <limits> 00041 #include <iostream> 00042 00043 #include <boost/math/constants/constants.hpp> 00044 00045 #include <ompl/datastructures/BinaryHeap.h> 00046 #include <ompl/tools/config/SelfConfig.h> 00047 #include <ompl/base/objectives/PathLengthOptimizationObjective.h> 00048 #include <ompl/geometric/planners/fmt/FMT.h> 00049 00050 00051 ompl::geometric::FMT::FMT(const base::SpaceInformationPtr &si) 00052 : base::Planner(si, "FMT") 00053 , numSamples_(1000) 00054 , radiusMultiplier_(1.1) 00055 { 00056 freeSpaceVolume_ = std::pow(si_->getMaximumExtent() / std::sqrt(si_->getStateDimension()), (int)si_->getStateDimension()); 00057 lastGoalMotion_ = NULL; 00058 00059 specs_.approximateSolutions = false; 00060 specs_.directed = false; 00061 00062 ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &FMT::setNumSamples, &FMT::getNumSamples, "10:10:10000"); 00063 ompl::base::Planner::declareParam<double>("radius_multiplier", this, &FMT::setRadiusMultiplier, &FMT::getRadiusMultiplier, "0.9:0.05:5."); 00064 ompl::base::Planner::declareParam<double>("free_space_volume", this, &FMT::setFreeSpaceVolume, &FMT::getFreeSpaceVolume, "1.:10:1000000."); 00065 } 00066 00067 ompl::geometric::FMT::~FMT() 00068 { 00069 freeMemory(); 00070 } 00071 00072 void ompl::geometric::FMT::setup() 00073 { 00074 Planner::setup(); 00075 00076 /* Setup the optimization objective. If no optimization objective was 00077 specified, then default to optimizing path length as computed by the 00078 distance() function in the state space */ 00079 if (pdef_->hasOptimizationObjective()) 00080 opt_ = pdef_->getOptimizationObjective(); 00081 else 00082 { 00083 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", getName().c_str()); 00084 opt_.reset(new base::PathLengthOptimizationObjective(si_)); 00085 } 00086 H_.getComparisonOperator().opt_ = opt_.get(); 00087 00088 if (!nn_) 00089 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace())); 00090 nn_->setDistanceFunction(boost::bind(&FMT::distanceFunction, this, _1, _2)); 00091 } 00092 00093 void ompl::geometric::FMT::freeMemory() 00094 { 00095 if (nn_) 00096 { 00097 std::vector<Motion*> motions; 00098 motions.reserve(nn_->size()); 00099 nn_->list(motions); 00100 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00101 { 00102 si_->freeState(motions[i]->getState()); 00103 delete motions[i]; 00104 } 00105 } 00106 } 00107 00108 void ompl::geometric::FMT::clear() 00109 { 00110 Planner::clear(); 00111 lastGoalMotion_ = NULL; 00112 sampler_.reset(); 00113 freeMemory(); 00114 if (nn_) 00115 nn_->clear(); 00116 H_.clear(); 00117 hElements_.clear(); 00118 neighborhoods_.clear(); 00119 } 00120 00121 void ompl::geometric::FMT::getPlannerData(base::PlannerData &data) const 00122 { 00123 Planner::getPlannerData(data); 00124 std::vector<Motion*> motions; 00125 nn_->list(motions); 00126 00127 if (lastGoalMotion_) 00128 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->getState())); 00129 00130 unsigned int size = motions.size(); 00131 for (unsigned int i = 0; i < size; ++i) 00132 { 00133 if (motions[i]->getParent() == NULL) 00134 data.addStartVertex(base::PlannerDataVertex(motions[i]->getState())); 00135 else 00136 data.addEdge(base::PlannerDataVertex(motions[i]->getParent()->getState()), 00137 base::PlannerDataVertex(motions[i]->getState())); 00138 } 00139 } 00140 00141 void ompl::geometric::FMT::saveNeighborhood(Motion *m, const double r) 00142 { 00143 // Check to see if neighborhood has not been saved yet 00144 if (neighborhoods_.find(m) == neighborhoods_.end()) 00145 { 00146 std::vector<Motion*> nbh; 00147 nn_->nearestR(m, r, nbh); 00148 if (!nbh.empty()) 00149 { 00150 // Save the neighborhood but skip the first element, since it will be motion m 00151 neighborhoods_[m] = std::vector<Motion*>(nbh.size() - 1, 0); 00152 std::copy(nbh.begin() + 1, nbh.end(), neighborhoods_[m].begin()); 00153 } 00154 else 00155 { 00156 // Save an empty neighborhood 00157 neighborhoods_[m] = std::vector<Motion*>(0); 00158 } 00159 } // If neighborhood hadn't been saved yet 00160 } 00161 00162 // Calculate the unit ball volume for a given dimension 00163 double ompl::geometric::FMT::calculateUnitBallVolume(const unsigned int dimension) const 00164 { 00165 if (dimension == 0) 00166 return 1.0; 00167 else if (dimension == 1) 00168 return 2.0; 00169 return 2.0 * boost::math::constants::pi<double>() / dimension 00170 * calculateUnitBallVolume(dimension - 2); 00171 } 00172 00173 double ompl::geometric::FMT::calculateRadius(const unsigned int dimension, const unsigned int n) const 00174 { 00175 double a = 1.0 / (double)dimension; 00176 double unitBallVolume = calculateUnitBallVolume(dimension); 00177 00178 return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) * std::pow(log((double)n) / (double)n, a); 00179 } 00180 00181 void ompl::geometric::FMT::sampleFree(const base::PlannerTerminationCondition &ptc) 00182 { 00183 unsigned int nodeCount = 0; 00184 Motion *motion = new Motion(si_); 00185 00186 // Sample numSamples_ number of nodes from the free configuration space 00187 while (nodeCount < numSamples_ && !ptc) 00188 { 00189 sampler_->sampleUniform(motion->getState()); 00190 00191 bool collision_free = si_->isValid(motion->getState()); 00192 00193 if (collision_free) 00194 { 00195 nodeCount++; 00196 nn_->add(motion); 00197 motion = new Motion(si_); 00198 } // If collision free 00199 } // While nodeCount < numSamples 00200 si_->freeState(motion->getState()); 00201 delete motion; 00202 } 00203 00204 void ompl::geometric::FMT::assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal) 00205 { 00206 // Ensure that there is at least one node near each goal 00207 while (const base::State *goalState = pis_.nextGoal()) 00208 { 00209 Motion *gMotion = new Motion(si_); 00210 si_->copyState(gMotion->getState(), goalState); 00211 00212 std::vector<Motion*> nearGoal; 00213 nn_->nearestR(gMotion, goal->getThreshold(), nearGoal); 00214 00215 // If there is no node in the goal region, insert one 00216 if (nearGoal.empty()) 00217 { 00218 OMPL_DEBUG("No state inside goal region"); 00219 if (si_->getStateValidityChecker()->isValid(gMotion->getState())) 00220 { 00221 gMotion->setSetType(Motion::SET_W); 00222 nn_->add(gMotion); 00223 } 00224 else 00225 { 00226 si_->freeState(gMotion->getState()); 00227 delete gMotion; 00228 } 00229 } 00230 else // There is already a sample in the goal region 00231 { 00232 si_->freeState(gMotion->getState()); 00233 delete gMotion; 00234 } 00235 } // For each goal 00236 } 00237 00238 ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTerminationCondition &ptc) 00239 { 00240 if (lastGoalMotion_) { 00241 OMPL_INFORM("solve() called before clear(); returning previous solution"); 00242 traceSolutionPathThroughTree(lastGoalMotion_); 00243 OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value()); 00244 return base::PlannerStatus(true, false); 00245 } 00246 else if (hElements_.size() > 0) 00247 { 00248 OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh"); 00249 clear(); 00250 } 00251 00252 checkValidity(); 00253 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00254 Motion *initMotion = NULL; 00255 00256 if (!goal) 00257 { 00258 OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); 00259 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; 00260 } 00261 00262 // Add start states to V (nn_) and H 00263 while (const base::State *st = pis_.nextStart()) 00264 { 00265 initMotion = new Motion(si_); 00266 si_->copyState(initMotion->getState(), st); 00267 hElements_[initMotion] = H_.insert(initMotion); 00268 initMotion->setSetType(Motion::SET_H); 00269 initMotion->setCost(opt_->initialCost(initMotion->getState())); 00270 nn_->add(initMotion); // V <-- {x_init} 00271 } 00272 00273 if (!initMotion) 00274 { 00275 OMPL_ERROR("Start state undefined"); 00276 return base::PlannerStatus::INVALID_START; 00277 } 00278 00279 // Sample N free states in the configuration space 00280 if (!sampler_) 00281 sampler_ = si_->allocStateSampler(); 00282 sampleFree(ptc); 00283 assureGoalIsSampled(goal); 00284 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size()); 00285 00286 // Calculate the nearest neighbor search radius 00287 double r = calculateRadius(si_->getStateDimension(), nn_->size()); 00288 OMPL_DEBUG("Using radius of %f", r); 00289 00290 // Flag all nodes as in set W 00291 std::vector<Motion*> vNodes; 00292 vNodes.reserve(nn_->size()); 00293 nn_->list(vNodes); 00294 unsigned int vNodesSize = vNodes.size(); 00295 for (unsigned int i = 0; i < vNodesSize; ++i) 00296 { 00297 vNodes[i]->setSetType(Motion::SET_W); 00298 } 00299 00300 // Execute the planner, and return early if the planner returns a failure 00301 bool plannerSuccess = false; 00302 bool successfulExpansion = false; 00303 Motion *z = initMotion; // z <-- xinit 00304 z->setSetType(Motion::SET_H); 00305 saveNeighborhood(z, r); 00306 00307 while (!ptc && !(plannerSuccess = goal->isSatisfied(z->getState()))) 00308 { 00309 successfulExpansion = expandTreeFromNode(z, r); 00310 if (!successfulExpansion) 00311 return base::PlannerStatus(false, false); 00312 } // While not at goal 00313 00314 if (plannerSuccess) 00315 { 00316 // Return the path to z, since by definition of planner success, z is in the goal region 00317 lastGoalMotion_ = z; 00318 traceSolutionPathThroughTree(lastGoalMotion_); 00319 00320 OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value()); 00321 return base::PlannerStatus(true, false); 00322 } // if plannerSuccess 00323 else 00324 { 00325 // Planner terminated without accomplishing goal 00326 return base::PlannerStatus(false, false); 00327 } 00328 } 00329 00330 void ompl::geometric::FMT::traceSolutionPathThroughTree(Motion *goalMotion) 00331 { 00332 std::vector<Motion*> mpath; 00333 Motion *solution = goalMotion; 00334 00335 // Construct the solution path 00336 while (solution != NULL) 00337 { 00338 mpath.push_back(solution); 00339 solution = solution->getParent(); 00340 } 00341 00342 // Set the solution path 00343 PathGeometric *path = new PathGeometric(si_); 00344 int mPathSize = mpath.size(); 00345 for (int i = mPathSize - 1 ; i >= 0 ; --i) 00346 path->append(mpath[i]->getState()); 00347 pdef_->addSolutionPath(base::PathPtr(path), false, -1.0, getName()); 00348 } 00349 00350 bool ompl::geometric::FMT::expandTreeFromNode(Motion *&z, const double r) 00351 { 00352 // Find all nodes that are near z, and also in set W 00353 00354 std::vector<Motion*> xNear; 00355 const std::vector<Motion*> &zNeighborhood = neighborhoods_[z]; 00356 unsigned int zNeighborhoodSize = zNeighborhood.size(); 00357 xNear.reserve(zNeighborhoodSize); 00358 00359 for (unsigned int i = 0; i < zNeighborhoodSize; ++i) 00360 { 00361 if (zNeighborhood[i]->getSetType() == Motion::SET_W) 00362 xNear.push_back(zNeighborhood[i]); 00363 } 00364 00365 // For each node near z and in set W, attempt to connect it to set H 00366 std::vector<Motion*> yNear; 00367 std::vector<Motion*> H_new; 00368 unsigned int xNearSize = xNear.size(); 00369 for (unsigned int i = 0 ; i < xNearSize; ++i) 00370 { 00371 Motion *x = xNear[i]; 00372 00373 // Find all nodes that are near x and in set H 00374 saveNeighborhood(x,r); 00375 const std::vector<Motion*> &xNeighborhood = neighborhoods_[x]; 00376 00377 unsigned int xNeighborhoodSize = xNeighborhood.size(); 00378 yNear.reserve(xNeighborhoodSize); 00379 for (unsigned int j = 0; j < xNeighborhoodSize; ++j) 00380 { 00381 if (xNeighborhood[j]->getSetType() == Motion::SET_H) 00382 yNear.push_back(xNeighborhood[j]); 00383 } 00384 00385 // Find the lowest cost-to-come connection from H to x 00386 Motion *yMin = NULL; 00387 base::Cost cMin(std::numeric_limits<double>::infinity()); 00388 unsigned int yNearSize = yNear.size(); 00389 for (unsigned int j = 0; j < yNearSize; ++j) 00390 { 00391 base::State *yState = yNear[j]->getState(); 00392 base::Cost dist = opt_->motionCost(yState, x->getState()); 00393 base::Cost cNew = opt_->combineCosts(yNear[j]->getCost(), dist); 00394 00395 if (opt_->isCostBetterThan(cNew, cMin)) 00396 { 00397 yMin = yNear[j]; 00398 cMin = cNew; 00399 } 00400 } 00401 yNear.clear(); 00402 00403 // If an optimal connection from H to x was found 00404 if (yMin != NULL) 00405 { 00406 bool collision_free = si_->checkMotion(yMin->getState(), x->getState()); 00407 00408 if (collision_free) 00409 { 00410 // Add edge from yMin to x 00411 x->setParent(yMin); 00412 x->setCost(cMin); 00413 // Add x to H_new 00414 H_new.push_back(x); 00415 // Remove x from W 00416 x->setSetType(Motion::SET_NULL); 00417 } 00418 } // An optimal connection from H to x was found 00419 } // For each node near z and in set W, try to connect it to set H 00420 00421 // Remove motion z from the binary heap and from the map 00422 H_.remove(hElements_[z]); 00423 hElements_.erase(z); 00424 z->setSetType(Motion::SET_NULL); 00425 00426 // Add the nodes in H_new to H 00427 unsigned int hNewSize = H_new.size(); 00428 for (unsigned int i = 0; i < hNewSize; i++) 00429 { 00430 hElements_[H_new[i]] = H_.insert(H_new[i]); 00431 H_new[i]->setSetType(Motion::SET_H); 00432 } 00433 00434 H_new.clear(); 00435 00436 if (H_.empty()) 00437 { 00438 OMPL_INFORM("H is empty before path was found --> no feasible path exists"); 00439 return false; 00440 } 00441 00442 // Take the top of H as the new z 00443 z = H_.top()->data; 00444 00445 return true; 00446 }