All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
BallTreeRRTstar.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice 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 the Rice 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: Alejandro Perez, Sertac Karaman, Ioan Sucan */
00036 
00037 #include "ompl/contrib/rrt_star/BallTreeRRTstar.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <algorithm>
00042 #include <limits>
00043 #include <map>
00044 
00045 ompl::geometric::BallTreeRRTstar::BallTreeRRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "BallTreeRRTstar")
00046 {
00047     specs_.approximateSolutions = true;
00048     specs_.optimizingPaths = true;
00049 
00050     goalBias_ = 0.05;
00051     maxDistance_ = 0.0;
00052     ballRadiusMax_ = 0.0;
00053     ballRadiusConst_ = 1.0;
00054     rO_ = std::numeric_limits<double>::infinity();
00055     delayCC_ = true;
00056 
00057     Planner::declareParam<double>("range", this, &BallTreeRRTstar::setRange, &BallTreeRRTstar::getRange);
00058     Planner::declareParam<double>("goal_bias", this, &BallTreeRRTstar::setGoalBias, &BallTreeRRTstar::getGoalBias);
00059     Planner::declareParam<double>("ball_radius_constant", this, &BallTreeRRTstar::setBallRadiusConstant, &BallTreeRRTstar::getBallRadiusConstant);
00060     Planner::declareParam<double>("max_ball_radius", this, &BallTreeRRTstar::setMaxBallRadius, &BallTreeRRTstar::getMaxBallRadius);
00061     Planner::declareParam<double>("initial_volume_radius", this, &BallTreeRRTstar::setInitialVolumeRadius, &BallTreeRRTstar::getInitialVolumeRadius);
00062     Planner::declareParam<bool>("delay_cc", this, &BallTreeRRTstar::setDelayCC, &BallTreeRRTstar::getDelayCC);
00063 }
00064 
00065 ompl::geometric::BallTreeRRTstar::~BallTreeRRTstar(void)
00066 {
00067     freeMemory();
00068 }
00069 
00070 void ompl::geometric::BallTreeRRTstar::setup(void)
00071 {
00072     Planner::setup();
00073     tools::SelfConfig sc(si_, getName());
00074     sc.configurePlannerRange(maxDistance_);
00075 
00076     ballRadiusMax_ = si_->getMaximumExtent();
00077     ballRadiusConst_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
00078 
00079     delayCC_ = true;
00080 
00081     if (!nn_)
00082         nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
00083     nn_->setDistanceFunction(boost::bind(&BallTreeRRTstar::distanceFunction, this, _1, _2));
00084 }
00085 
00086 void ompl::geometric::BallTreeRRTstar::clear(void)
00087 {
00088     Planner::clear();
00089     sampler_.reset();
00090     motions_.clear();
00091     freeMemory();
00092     if (nn_)
00093         nn_->clear();
00094 }
00095 
00096 bool ompl::geometric::BallTreeRRTstar::solve(const base::PlannerTerminationCondition &ptc)
00097 {
00098     checkValidity();
00099     base::Goal                 *goal   = pdef_->getGoal().get();
00100     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00101 
00102     if (!goal)
00103     {
00104         msg_.error("Goal undefined");
00105         return false;
00106     }
00107 
00108     while (const base::State *st = pis_.nextStart())
00109     {
00110         Motion *motion = new Motion(si_, rO_);
00111         si_->copyState(motion->state, st);
00112         addMotion(motion);
00113     }
00114 
00115     if (nn_->size() == 0)
00116     {
00117         msg_.error("There are no valid initial states!");
00118         return false;
00119     }
00120 
00121     if (!sampler_)
00122         sampler_ = si_->allocStateSampler();
00123 
00124     msg_.inform("Starting with %u states", nn_->size());
00125 
00126     Motion *solution       = NULL;
00127     Motion *approximation  = NULL;
00128     double approximatedist = std::numeric_limits<double>::infinity();
00129     bool sufficientlyShort = false;
00130 
00131     Motion *rmotion   = new Motion(si_, rO_);
00132     Motion *toTrim    = NULL;
00133     base::State *rstate = rmotion->state;
00134     base::State *xstate = si_->allocState();
00135     base::State *tstate = si_->allocState();
00136     std::vector<Motion*> solCheck;
00137     std::vector<Motion*> nbh;
00138     std::vector<double>  dists;
00139     std::vector<int>     valid;
00140     long unsigned int    rewireTest = 0;
00141 
00142     std::pair<base::State*,double> lastValid(tstate, 0.0);
00143 
00144     while (ptc() == false)
00145     {
00146         bool rejected = false;
00147 
00148         /* sample until a state not within any of the existing volumes is found */
00149         do
00150         {
00151             /* sample random state (with goal biasing) */
00152             if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00153                 goal_s->sampleGoal(rstate);
00154             else
00155                 sampler_->sampleUniform(rstate);
00156 
00157             /* check to see if it is inside an existing volume */
00158             if (inVolume(rstate))
00159             {
00160                 rejected = true;
00161 
00162                 /* see if the state is valid */
00163                 if(!si_->isValid(rstate))
00164                 {
00165                     /* if it's not, reduce the size of the nearest volume to the distance
00166                        between its center and the rejected state */
00167                     toTrim = nn_->nearest(rmotion);
00168                     double newRad = si_->distance(toTrim->state, rstate);
00169                     if (newRad < toTrim->volRadius)
00170                         toTrim->volRadius = newRad;
00171                 }
00172 
00173             }
00174             else
00175 
00176                 rejected = false;
00177 
00178         }
00179         while (rejected);
00180 
00181         /* find closest state in the tree */
00182         Motion *nmotion = nn_->nearest(rmotion);
00183 
00184         base::State *dstate = rstate;
00185 
00186         /* find state to add */
00187         double d = si_->distance(nmotion->state, rstate);
00188         if (d > maxDistance_)
00189         {
00190             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00191             dstate = xstate;
00192         }
00193 
00194         if (si_->checkMotion(nmotion->state, dstate, lastValid))
00195         {
00196             /* create a motion */
00197             double distN = si_->distance(dstate, nmotion->state);
00198             Motion *motion = new Motion(si_, rO_);
00199             si_->copyState(motion->state, dstate);
00200             motion->parent = nmotion;
00201             motion->cost = nmotion->cost + distN;
00202 
00203             /* find nearby neighbors */
00204             double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
00205                                 ballRadiusMax_);
00206 
00207             nn_->nearestR(motion, r, nbh);
00208             rewireTest += nbh.size();
00209 
00210             // cache for distance computations
00211             dists.resize(nbh.size());
00212             // cache for motion validity
00213             valid.resize(nbh.size());
00214             std::fill(valid.begin(), valid.end(), 0);
00215 
00216             if (delayCC_)
00217             {
00218                 // calculate all costs and distances
00219                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00220                     if (nbh[i] != nmotion)
00221                     {
00222                         double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
00223                         nbh[i]->cost = c;
00224                     }
00225 
00226                 // sort the nodes
00227                 std::sort(nbh.begin(), nbh.end(), compareMotion);
00228 
00229                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00230                     if (nbh[i] != nmotion)
00231                     {
00232                         dists[i] = si_->distance(nbh[i]->state, dstate);
00233                         nbh[i]->cost -= dists[i];
00234                     }
00235                 // collision check until a valid motion is found
00236                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00237                     if (nbh[i] != nmotion)
00238                     {
00239 
00240                         dists[i] = si_->distance(nbh[i]->state, dstate);
00241                         double c = nbh[i]->cost + dists[i];
00242                         if (c < motion->cost)
00243                         {
00244                             if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
00245                             {
00246                                 motion->cost = c;
00247                                 motion->parent = nbh[i];
00248                                 valid[i] = 1;
00249                                 break;
00250                             }
00251                             else
00252                             {
00253                                 valid[i] = -1;
00254                                 /* if a collision is found, trim radius to distance from motion to last valid state */
00255                                 double nR = si_->distance(nbh[i]->state, lastValid.first);
00256                                 if (nR < nbh[i]->volRadius)
00257                                     nbh[i]->volRadius = nR;
00258                             }
00259                         }
00260                     }
00261                     else
00262                     {
00263                         valid[i] = 1;
00264                         dists[i] = distN;
00265                         break;
00266                     }
00267 
00268             }
00269             else{
00270                 /* find which one we connect the new state to*/
00271                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00272                     if (nbh[i] != nmotion)
00273                     {
00274 
00275                         dists[i] = si_->distance(nbh[i]->state, dstate);
00276                         double c = nbh[i]->cost + dists[i];
00277                         if (c < motion->cost)
00278                         {
00279                             if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
00280                             {
00281                                 motion->cost = c;
00282                                 motion->parent = nbh[i];
00283                                 valid[i] = 1;
00284                             }
00285                             else
00286                             {
00287                                 valid[i] = -1;
00288                                 /* if a collision is found, trim radius to distance from motion to last valid state */
00289                                 double newR = si_->distance(nbh[i]->state, lastValid.first);
00290                                 if (newR < nbh[i]->volRadius)
00291                                     nbh[i]->volRadius = newR;
00292 
00293                             }
00294                         }
00295                     }
00296                     else
00297                     {
00298                         valid[i] = 1;
00299                         dists[i] = distN;
00300                     }
00301             }
00302 
00303             /* add motion to tree */
00304             addMotion(motion);
00305             motion->parent->children.push_back(motion);
00306 
00307             solCheck.resize(1);
00308             solCheck[0] = motion;
00309 
00310             /* rewire tree if needed */
00311             for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00312                 if (nbh[i] != motion->parent)
00313                 {
00314                     double c = motion->cost + dists[i];
00315                     if (c < nbh[i]->cost)
00316                     {
00317                         bool v = false;
00318                         if (valid[i] == 0)
00319                         {
00320                             if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
00321                             {
00322                                 /* if a collision is found, trim radius to distance from motion to last valid state */
00323                                 double R =  si_->distance(nbh[i]->state, lastValid.first);
00324                                 if (R < nbh[i]->volRadius)
00325                                     nbh[i]->volRadius = R;
00326                             }
00327                             else
00328                             {
00329                                 v = true;
00330                             }
00331                         }
00332                         if (valid[i] == 1)
00333                             v = true;
00334 
00335                         if (v)
00336                         {
00337                             // Remove this node from its parent list
00338                             removeFromParent (nbh[i]);
00339                             double delta = c - nbh[i]->cost;
00340 
00341                             nbh[i]->parent = motion;
00342                             nbh[i]->cost = c;
00343                             nbh[i]->parent->children.push_back(nbh[i]);
00344                             solCheck.push_back(nbh[i]);
00345 
00346                             // Update the costs of the node's children
00347                             updateChildCosts(nbh[i], delta);
00348                         }
00349                     }
00350                 }
00351 
00352             // check if we found a solution
00353             for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00354             {
00355                 double dist = 0.0;
00356                 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00357                 sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00358 
00359                 if (solved)
00360                 {
00361                     if (sufficientlyShort)
00362                     {
00363                         solution = solCheck[i];
00364                         break;
00365                     }
00366                     else if (!solution || (solCheck[i]->cost < solution->cost))
00367                     {
00368                         solution = solCheck[i];
00369                     }
00370                 }
00371                 else if (!solution && dist < approximatedist)
00372                 {
00373                     approximation = solCheck[i];
00374                     approximatedist = dist;
00375                 }
00376             }
00377 
00378             // terminate if a sufficient solution is found
00379             if (solution && sufficientlyShort)
00380                 break;
00381         }
00382         else
00383         {
00384             /* if a collision is found, trim radius to distance from motion to last valid state */
00385             toTrim = nn_->nearest(nmotion);
00386             double newRadius =  si_->distance(toTrim->state, lastValid.first);
00387             if (newRadius < toTrim->volRadius)
00388                 toTrim->volRadius = newRadius;
00389         }
00390     }
00391 
00392     double solutionCost;
00393     bool approximate = (solution == NULL);
00394     bool addedSolution = false;
00395     if (approximate)
00396     {
00397         solution = approximation;
00398         solutionCost = approximatedist;
00399     }
00400     else
00401         solutionCost = solution->cost;
00402 
00403     if (solution != NULL)
00404     {
00405         // construct the solution path
00406         std::vector<Motion*> mpath;
00407         while (solution != NULL)
00408         {
00409             mpath.push_back(solution);
00410             solution = solution->parent;
00411         }
00412 
00413         // set the solution path
00414         PathGeometric *path = new PathGeometric(si_);
00415         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00416             path->append(mpath[i]->state);
00417         goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
00418         addedSolution = true;
00419     }
00420 
00421     si_->freeState(xstate);
00422     if (rmotion->state)
00423         si_->freeState(rmotion->state);
00424     delete rmotion;
00425 
00426     msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00427 
00428     return addedSolution;
00429 }
00430 
00431 void ompl::geometric::BallTreeRRTstar::removeFromParent(Motion *m)
00432 {
00433     std::vector<Motion*>::iterator it = m->parent->children.begin ();
00434     while (it != m->parent->children.end ())
00435     {
00436         if (*it == m)
00437         {
00438             it = m->parent->children.erase(it);
00439             it = m->parent->children.end ();
00440         }
00441         else
00442             ++it;
00443     }
00444 }
00445 
00446 void ompl::geometric::BallTreeRRTstar::updateChildCosts(Motion *m, double delta)
00447 {
00448     for (size_t i = 0; i < m->children.size(); ++i)
00449     {
00450         m->children[i]->cost += delta;
00451         updateChildCosts(m->children[i], delta);
00452     }
00453 }
00454 
00455 void ompl::geometric::BallTreeRRTstar::freeMemory(void)
00456 {
00457     if (nn_)
00458     {
00459         std::vector<Motion*> motions;
00460         nn_->list(motions);
00461         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00462         {
00463             if (motions[i]->state)
00464                 si_->freeState(motions[i]->state);
00465             delete motions[i];
00466         }
00467     }
00468 }
00469 
00470 void ompl::geometric::BallTreeRRTstar::getPlannerData(base::PlannerData &data) const
00471 {
00472     Planner::getPlannerData(data);
00473 
00474     std::vector<Motion*> motions;
00475     if (nn_)
00476         nn_->list(motions);
00477 
00478     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00479         data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00480 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends