ompl/geometric/planners/rrt/src/LBTRRT.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Oren Salzman
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 the Willow Garage 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 /* Author: Oren Salzman, Sertac Karaman, Ioan Sucan, Mark Moll */
00036 
00037 #include "ompl/geometric/planners/rrt/LBTRRT.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00041 #include <limits>
00042 #include <math.h>
00043 
00044 const double ompl::geometric::LBTRRT::kRRG = 5.5;
00045 
00046 ompl::geometric::LBTRRT::LBTRRT(const base::SpaceInformationPtr &si) :
00047     base::Planner(si, "LBTRRT"),
00048     goalBias_(0.05),
00049     maxDistance_(0.0),
00050     epsilon_(0.4),
00051     lastGoalMotion_(NULL),
00052     iterations_(0),
00053     bestCost_(std::numeric_limits<double>::quiet_NaN())
00054     {
00055 
00056     specs_.approximateSolutions = true;
00057     specs_.directed = true;
00058 
00059     Planner::declareParam<double>("range", this, &LBTRRT::setRange, &LBTRRT::getRange, "0.:1.:10000.");
00060     Planner::declareParam<double>("goal_bias", this, &LBTRRT::setGoalBias, &LBTRRT::getGoalBias, "0.:.05:1.");
00061     Planner::declareParam<double>("epsilon", this, &LBTRRT::setApproximationFactor, &LBTRRT::getApproximationFactor, "0.:.1:10.");
00062 
00063     addPlannerProgressProperty("iterations INTEGER",
00064                                boost::bind(&LBTRRT::getIterationCount, this));
00065     addPlannerProgressProperty("best cost REAL",
00066                                boost::bind(&LBTRRT::getBestCost, this));
00067 
00068 }
00069 
00070 ompl::geometric::LBTRRT::~LBTRRT()
00071 {
00072     freeMemory();
00073 }
00074 
00075 void ompl::geometric::LBTRRT::clear()
00076 {
00077     Planner::clear();
00078     sampler_.reset();
00079     freeMemory();
00080     if (nn_)
00081         nn_->clear();
00082     lastGoalMotion_ = NULL;
00083     goalMotions_.clear();
00084 
00085     iterations_ = 0;
00086     bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
00087 }
00088 
00089 void ompl::geometric::LBTRRT::setup()
00090 {
00091     Planner::setup();
00092     tools::SelfConfig sc(si_, getName());
00093     sc.configurePlannerRange(maxDistance_);
00094 
00095     if (!nn_)
00096         nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
00097     nn_->setDistanceFunction(boost::bind(&LBTRRT::distanceFunction, this, _1, _2));
00098 
00099     // Setup optimization objective
00100     //
00101     // If no optimization objective was specified, then default to
00102     // optimizing path length as computed by the distance() function
00103     // in the state space.
00104     if (pdef_)
00105     {
00106         if (pdef_->hasOptimizationObjective())
00107         {
00108             opt_ = pdef_->getOptimizationObjective();
00109             if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt_.get()))
00110                 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str());
00111         }
00112         else
00113             opt_.reset(new base::PathLengthOptimizationObjective(si_));
00114     }
00115     else
00116     {
00117         OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
00118         setup_ = false;
00119     }
00120 }
00121 
00122 void ompl::geometric::LBTRRT::freeMemory()
00123 {
00124     if (nn_)
00125     {
00126         std::vector<Motion*> motions;
00127         nn_->list(motions);
00128         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00129         {
00130             if (motions[i]->state)
00131                 si_->freeState(motions[i]->state);
00132             delete motions[i];
00133         }
00134     }
00135 }
00136 
00137 ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerminationCondition &ptc)
00138 {
00139     checkValidity();
00140     base::Goal                 *goal   = pdef_->getGoal().get();
00141     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00142 
00143     while (const base::State *st = pis_.nextStart())
00144     {
00145         Motion *motion = new Motion(si_);
00146         si_->copyState(motion->state, st);
00147         motion->costLb_ = motion->costApx_ = opt_->identityCost();
00148         nn_->add(motion);
00149     }
00150 
00151     if (nn_->size() == 0)
00152     {
00153         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00154         return base::PlannerStatus::INVALID_START;
00155     }
00156 
00157     if (!sampler_)
00158         sampler_ = si_->allocStateSampler();
00159 
00160     OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
00161 
00162     Motion *solution             = lastGoalMotion_;
00163 
00164     // \TODO Make this variable unnecessary, or at least have it
00165     // persist across solve runs
00166     base::Cost bestCost          = opt_->infiniteCost();
00167     Motion *approximation        = NULL;
00168 
00169     double  approximatedist      = std::numeric_limits<double>::infinity();
00170     bool sufficientlyShort       = false;
00171 
00172     Motion *rmotion              = new Motion(si_);
00173     base::State *rstate          = rmotion->state;
00174     base::State *xstate          = si_->allocState();
00175     unsigned int statesGenerated = 0;
00176 
00177     while (ptc() == false)
00178     {
00179         iterations_++;
00180         /* sample random state (with goal biasing) */
00181         // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal states.
00182         if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && goal_s->canSample())
00183             goal_s->sampleGoal(rstate);
00184         else
00185             sampler_->sampleUniform(rstate);
00186 
00187         /* find closest state in the tree */
00188         Motion *nmotion = nn_->nearest(rmotion);
00189         base::State *dstate = rstate;
00190 
00191         /* find state to add */
00192         double d = si_->distance(nmotion->state, rstate);
00193         if (d > maxDistance_)
00194         {
00195             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00196             dstate = xstate;
00197         }
00198 
00199         if (si_->checkMotion(nmotion->state, dstate))
00200         {
00201             statesGenerated++;
00202             /* create a motion */
00203             Motion *motion = new Motion(si_);
00204             si_->copyState(motion->state, dstate);
00205 
00206             /* update fields */
00207             motion->parentLb_ = nmotion;
00208             motion->parentApx_ = nmotion;
00209             motion->incCost_ = costFunction(nmotion, motion);
00210             motion->costLb_ = opt_->combineCosts(nmotion->costLb_, motion->incCost_);
00211             motion->costApx_ = opt_->combineCosts(nmotion->costApx_, motion->incCost_);
00212 
00213             nmotion->childrenLb_.push_back(motion);
00214             nmotion->childrenApx_.push_back(motion);
00215 
00216             nn_->add(motion);
00217 
00218             bool checkForSolution = false;
00219             /* do lazy rewiring */
00220             unsigned int k = std::ceil(std::log(double(nn_->size())) * kRRG);
00221             std::vector<Motion *> nnVec;
00222             nn_->nearestK(rmotion, k, nnVec);
00223 
00224             CostCompare costCompare(*opt_, motion);
00225             std::sort(nnVec.begin(), nnVec.end(), costCompare);
00226 
00227             for (std::size_t i = 0; i < nnVec.size(); ++i)
00228                 checkForSolution |= attemptNodeUpdate(motion, nnVec[i]);
00229 
00230             for (std::size_t i = 0; i < nnVec.size(); ++i)
00231                 checkForSolution |= attemptNodeUpdate(nnVec[i], motion);
00232 
00233             double distanceFromGoal;
00234             if (goal->isSatisfied(motion->state, &distanceFromGoal))
00235             {
00236                 goalMotions_.push_back(motion);
00237                 checkForSolution = true;
00238             }
00239 
00240             // Checking for solution or iterative improvement
00241             if (checkForSolution)
00242             {
00243                 for (size_t i = 0; i < goalMotions_.size(); ++i)
00244                 {
00245                     if (opt_->isCostBetterThan(goalMotions_[i]->costApx_, bestCost))
00246                     {
00247                         bestCost = goalMotions_[i]->costApx_;
00248                         bestCost_ = bestCost;
00249                     }
00250 
00251                     sufficientlyShort = opt_->isSatisfied(goalMotions_[i]->costApx_);
00252                     if (sufficientlyShort)
00253                     {
00254                         solution = goalMotions_[i];
00255                         break;
00256                     }
00257                     else if (!solution ||
00258                              opt_->isCostBetterThan(goalMotions_[i]->costApx_, solution->costApx_))
00259                         solution = goalMotions_[i];
00260                 }
00261             }
00262 
00263             // Checking for approximate solution (closest state found to the goal)
00264             if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
00265             {
00266                 approximation = motion;
00267                 approximatedist = distanceFromGoal;
00268             }
00269         }
00270 
00271         // terminate if a sufficient solution is found
00272         if (solution && sufficientlyShort)
00273             break;
00274     }
00275 
00276     bool approximate = (solution == 0);
00277     bool addedSolution = false;
00278     if (approximate)
00279         solution = approximation;
00280     else
00281         lastGoalMotion_ = solution;
00282 
00283     if (solution != NULL)
00284     {
00285         /* construct the solution path */
00286         std::vector<Motion*> mpath;
00287         while (solution != NULL)
00288         {
00289             mpath.push_back(solution);
00290             solution = solution->parentApx_;
00291         }
00292 
00293         /* set the solution path */
00294         PathGeometric *geoPath = new PathGeometric(si_);
00295         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00296             geoPath->append(mpath[i]->state);
00297 
00298         base::PathPtr path(geoPath);
00299         // Add the solution path.
00300         base::PlannerSolution psol(path);
00301         psol.setPlannerName(getName());
00302         if (approximate)
00303             psol.setApproximate(approximatedist);
00304         // Does the solution satisfy the optimization objective?
00305         psol.setOptimized(opt_, bestCost, sufficientlyShort);
00306         pdef_->addSolutionPath(psol);
00307 
00308         addedSolution = true;
00309     }
00310 
00311     si_->freeState(xstate);
00312     if (rmotion->state)
00313         si_->freeState(rmotion->state);
00314     delete rmotion;
00315 
00316     OMPL_INFORM("%s: Created %u states. %u goal states in tree.", getName().c_str(), statesGenerated, goalMotions_.size());
00317 
00318     return base::PlannerStatus(addedSolution, approximate);
00319 }
00320 
00321 bool ompl::geometric::LBTRRT::attemptNodeUpdate(Motion *potentialParent, Motion *child)
00322 {
00323     base::Cost incCost = costFunction(potentialParent, child);
00324     base::Cost potentialLb = opt_->combineCosts(potentialParent->costLb_, incCost);
00325     base::Cost potentialApx = opt_->combineCosts(potentialParent->costApx_, incCost);
00326 
00327     if (!opt_->isCostBetterThan(potentialLb, child->costLb_))
00328         return false;
00329 
00330     if (opt_->isCostBetterThan(base::Cost((1.0 + epsilon_) *  potentialLb.value()), child->costApx_))
00331     {
00332         if (si_->checkMotion(potentialParent->state, child->state) == false)
00333             return false;
00334 
00335         removeFromParentLb(child);
00336         child->parentLb_ = potentialParent;
00337         potentialParent->childrenLb_.push_back(child);
00338         child->costLb_ = potentialLb;
00339         child->incCost_ = incCost;
00340         updateChildCostsLb(child);
00341 
00342 
00343         if (!opt_->isCostBetterThan(potentialApx, child->costApx_))
00344             return false;
00345 
00346         removeFromParentApx(child);
00347         child->parentApx_ = potentialParent;
00348         potentialParent->childrenApx_.push_back(child);
00349         child->costApx_ = potentialApx;
00350         updateChildCostsApx(child);
00351 
00352         if (opt_->isCostBetterThan(potentialApx, bestCost_))
00353             return true;
00354     }
00355     else //(child->costApx_ <= (1 + epsilon_) *  potentialLb)
00356     {
00357         removeFromParentLb(child);
00358         child->parentLb_ = potentialParent;
00359         potentialParent->childrenLb_.push_back(child);
00360         child->costLb_ = potentialLb;
00361         child->incCost_ = incCost;
00362         updateChildCostsLb(child);
00363     }
00364     return false;
00365 }
00366 
00367 void ompl::geometric::LBTRRT::getPlannerData(base::PlannerData &data) const
00368 {
00369     Planner::getPlannerData(data);
00370 
00371     std::vector<Motion*> motions;
00372     if (nn_)
00373         nn_->list(motions);
00374 
00375     if (lastGoalMotion_)
00376         data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
00377 
00378     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00379     {
00380         if (motions[i]->parentApx_ == NULL)
00381             data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
00382         else
00383             data.addEdge(base::PlannerDataVertex(motions[i]->parentApx_->state),
00384                          base::PlannerDataVertex(motions[i]->state));
00385     }
00386 }
00387 
00388 void ompl::geometric::LBTRRT::updateChildCostsLb(Motion *m)
00389 {
00390     for (std::size_t i = 0; i < m->childrenLb_.size(); ++i)
00391     {
00392         m->childrenLb_[i]->costLb_ = opt_->combineCosts(m->costLb_, m->childrenLb_[i]->incCost_);
00393         updateChildCostsLb(m->childrenLb_[i]);
00394     }
00395 }
00396 void ompl::geometric::LBTRRT::updateChildCostsApx(Motion *m)
00397 {
00398     for (std::size_t i = 0; i < m->childrenApx_.size(); ++i)
00399     {
00400         m->childrenApx_[i]->costApx_ = opt_->combineCosts(m->costApx_, m->childrenApx_[i]->incCost_);
00401         updateChildCostsApx(m->childrenApx_[i]);
00402     }
00403 }
00404 
00405 void ompl::geometric::LBTRRT::removeFromParentLb(Motion *m)
00406 {
00407     return removeFromParent(m, m->parentLb_->childrenLb_);
00408 }
00409 void ompl::geometric::LBTRRT::removeFromParentApx(Motion *m)
00410 {
00411     return removeFromParent(m, m->parentApx_->childrenApx_);
00412 }
00413 void ompl::geometric::LBTRRT::removeFromParent(const Motion *m, std::vector<Motion*>& vec)
00414 {
00415     for (std::vector<Motion*>::iterator it = vec.begin (); it != vec.end(); ++it)
00416         if (*it == m)
00417         {
00418             vec.erase(it);
00419             break;
00420         }
00421 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines