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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines