All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/contrib/rrt_star/src/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/goals/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 ompl::base::PlannerStatus 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         logError("Goal undefined");
00105         return base::PlannerStatus::INVALID_GOAL;
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         logError("There are no valid initial states!");
00118         return base::PlannerStatus::INVALID_START;
00119     }
00120 
00121     if (!sampler_)
00122         sampler_ = si_->allocStateSampler();
00123 
00124     logInform("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     double               stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
00142 
00143     std::pair<base::State*,double> lastValid(tstate, 0.0);
00144 
00145     while (ptc() == false)
00146     {
00147         bool rejected = false;
00148 
00149         /* sample until a state not within any of the existing volumes is found */
00150         do
00151         {
00152             /* sample random state (with goal biasing) */
00153             if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00154                 goal_s->sampleGoal(rstate);
00155             else
00156                 sampler_->sampleUniform(rstate);
00157 
00158             /* check to see if it is inside an existing volume */
00159             if (inVolume(rstate))
00160             {
00161                 rejected = true;
00162 
00163                 /* see if the state is valid */
00164                 if(!si_->isValid(rstate))
00165                 {
00166                     /* if it's not, reduce the size of the nearest volume to the distance
00167                        between its center and the rejected state */
00168                     toTrim = nn_->nearest(rmotion);
00169                     double newRad = si_->distance(toTrim->state, rstate);
00170                     if (newRad < toTrim->volRadius)
00171                         toTrim->volRadius = newRad;
00172                 }
00173 
00174             }
00175             else
00176 
00177                 rejected = false;
00178 
00179         }
00180         while (rejected);
00181 
00182         /* find closest state in the tree */
00183         Motion *nmotion = nn_->nearest(rmotion);
00184 
00185         base::State *dstate = rstate;
00186 
00187         /* find state to add */
00188         double d = si_->distance(nmotion->state, rstate);
00189         if (d > maxDistance_)
00190         {
00191             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00192             dstate = xstate;
00193         }
00194 
00195         if (si_->checkMotion(nmotion->state, dstate, lastValid))
00196         {
00197             /* create a motion */
00198             double distN = si_->distance(dstate, nmotion->state);
00199             Motion *motion = new Motion(si_, rO_);
00200             si_->copyState(motion->state, dstate);
00201             motion->parent = nmotion;
00202             motion->cost = nmotion->cost + distN;
00203 
00204             /* find nearby neighbors */
00205             double r = std::min(ballRadiusConst_ * pow(log((double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
00206                                 ballRadiusMax_);
00207 
00208             nn_->nearestR(motion, r, nbh);
00209             rewireTest += nbh.size();
00210 
00211             // cache for distance computations
00212             dists.resize(nbh.size());
00213             // cache for motion validity
00214             valid.resize(nbh.size());
00215             std::fill(valid.begin(), valid.end(), 0);
00216 
00217             if (delayCC_)
00218             {
00219                 // calculate all costs and distances
00220                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00221                     if (nbh[i] != nmotion)
00222                     {
00223                         double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
00224                         nbh[i]->cost = c;
00225                     }
00226 
00227                 // sort the nodes
00228                 std::sort(nbh.begin(), nbh.end(), compareMotion);
00229 
00230                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00231                     if (nbh[i] != nmotion)
00232                     {
00233                         dists[i] = si_->distance(nbh[i]->state, dstate);
00234                         nbh[i]->cost -= dists[i];
00235                     }
00236                 // collision check until a valid motion is found
00237                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00238                     if (nbh[i] != nmotion)
00239                     {
00240 
00241                         dists[i] = si_->distance(nbh[i]->state, dstate);
00242                         double c = nbh[i]->cost + dists[i];
00243                         if (c < motion->cost)
00244                         {
00245                             if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
00246                             {
00247                                 motion->cost = c;
00248                                 motion->parent = nbh[i];
00249                                 valid[i] = 1;
00250                                 break;
00251                             }
00252                             else
00253                             {
00254                                 valid[i] = -1;
00255                                 /* if a collision is found, trim radius to distance from motion to last valid state */
00256                                 double nR = si_->distance(nbh[i]->state, lastValid.first);
00257                                 if (nR < nbh[i]->volRadius)
00258                                     nbh[i]->volRadius = nR;
00259                             }
00260                         }
00261                     }
00262                     else
00263                     {
00264                         valid[i] = 1;
00265                         dists[i] = distN;
00266                         break;
00267                     }
00268 
00269             }
00270             else{
00271                 /* find which one we connect the new state to*/
00272                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00273                     if (nbh[i] != nmotion)
00274                     {
00275 
00276                         dists[i] = si_->distance(nbh[i]->state, dstate);
00277                         double c = nbh[i]->cost + dists[i];
00278                         if (c < motion->cost)
00279                         {
00280                             if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
00281                             {
00282                                 motion->cost = c;
00283                                 motion->parent = nbh[i];
00284                                 valid[i] = 1;
00285                             }
00286                             else
00287                             {
00288                                 valid[i] = -1;
00289                                 /* if a collision is found, trim radius to distance from motion to last valid state */
00290                                 double newR = si_->distance(nbh[i]->state, lastValid.first);
00291                                 if (newR < nbh[i]->volRadius)
00292                                     nbh[i]->volRadius = newR;
00293 
00294                             }
00295                         }
00296                     }
00297                     else
00298                     {
00299                         valid[i] = 1;
00300                         dists[i] = distN;
00301                     }
00302             }
00303 
00304             /* add motion to tree */
00305             addMotion(motion);
00306             motion->parent->children.push_back(motion);
00307 
00308             solCheck.resize(1);
00309             solCheck[0] = motion;
00310 
00311             /* rewire tree if needed */
00312             for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00313                 if (nbh[i] != motion->parent)
00314                 {
00315                     double c = motion->cost + dists[i];
00316                     if (c < nbh[i]->cost)
00317                     {
00318                         bool v = false;
00319                         if (valid[i] == 0)
00320                         {
00321                             if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
00322                             {
00323                                 /* if a collision is found, trim radius to distance from motion to last valid state */
00324                                 double R =  si_->distance(nbh[i]->state, lastValid.first);
00325                                 if (R < nbh[i]->volRadius)
00326                                     nbh[i]->volRadius = R;
00327                             }
00328                             else
00329                             {
00330                                 v = true;
00331                             }
00332                         }
00333                         if (valid[i] == 1)
00334                             v = true;
00335 
00336                         if (v)
00337                         {
00338                             // Remove this node from its parent list
00339                             removeFromParent (nbh[i]);
00340                             double delta = c - nbh[i]->cost;
00341 
00342                             nbh[i]->parent = motion;
00343                             nbh[i]->cost = c;
00344                             nbh[i]->parent->children.push_back(nbh[i]);
00345                             solCheck.push_back(nbh[i]);
00346 
00347                             // Update the costs of the node's children
00348                             updateChildCosts(nbh[i], delta);
00349                         }
00350                     }
00351                 }
00352 
00353             // Make sure to check the existing solution for improvement
00354             if (solution)
00355                 solCheck.push_back(solution);
00356 
00357             // check if we found a solution
00358             for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00359             {
00360                 double dist = 0.0;
00361                 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00362                 sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00363 
00364                 if (solved)
00365                 {
00366                     if (sufficientlyShort)
00367                     {
00368                         solution = solCheck[i];
00369                         break;
00370                     }
00371                     else if (!solution || (solCheck[i]->cost < solution->cost))
00372                     {
00373                         solution = solCheck[i];
00374                     }
00375                 }
00376                 else if (!solution && dist < approximatedist)
00377                 {
00378                     approximation = solCheck[i];
00379                     approximatedist = dist;
00380                 }
00381             }
00382 
00383             // terminate if a sufficient solution is found
00384             if (solution && sufficientlyShort)
00385                 break;
00386         }
00387         else
00388         {
00389             /* if a collision is found, trim radius to distance from motion to last valid state */
00390             toTrim = nn_->nearest(nmotion);
00391             double newRadius =  si_->distance(toTrim->state, lastValid.first);
00392             if (newRadius < toTrim->volRadius)
00393                 toTrim->volRadius = newRadius;
00394         }
00395     }
00396 
00397     double solutionCost;
00398     bool approximate = (solution == NULL);
00399     bool addedSolution = false;
00400     if (approximate)
00401     {
00402         solution = approximation;
00403         solutionCost = approximatedist;
00404     }
00405     else
00406         solutionCost = solution->cost;
00407 
00408     if (solution != NULL)
00409     {
00410         // construct the solution path
00411         std::vector<Motion*> mpath;
00412         while (solution != NULL)
00413         {
00414             mpath.push_back(solution);
00415             solution = solution->parent;
00416         }
00417 
00418         // set the solution path
00419         PathGeometric *path = new PathGeometric(si_);
00420         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00421             path->append(mpath[i]->state);
00422         pdef_->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
00423         addedSolution = true;
00424     }
00425 
00426     si_->freeState(xstate);
00427     if (rmotion->state)
00428         si_->freeState(rmotion->state);
00429     delete rmotion;
00430 
00431     logInform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00432 
00433     return base::PlannerStatus(addedSolution, approximate);
00434 }
00435 
00436 void ompl::geometric::BallTreeRRTstar::removeFromParent(Motion *m)
00437 {
00438     std::vector<Motion*>::iterator it = m->parent->children.begin ();
00439     while (it != m->parent->children.end ())
00440     {
00441         if (*it == m)
00442         {
00443             it = m->parent->children.erase(it);
00444             it = m->parent->children.end ();
00445         }
00446         else
00447             ++it;
00448     }
00449 }
00450 
00451 void ompl::geometric::BallTreeRRTstar::updateChildCosts(Motion *m, double delta)
00452 {
00453     for (size_t i = 0; i < m->children.size(); ++i)
00454     {
00455         m->children[i]->cost += delta;
00456         updateChildCosts(m->children[i], delta);
00457     }
00458 }
00459 
00460 void ompl::geometric::BallTreeRRTstar::freeMemory(void)
00461 {
00462     if (nn_)
00463     {
00464         std::vector<Motion*> motions;
00465         nn_->list(motions);
00466         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00467         {
00468             if (motions[i]->state)
00469                 si_->freeState(motions[i]->state);
00470             delete motions[i];
00471         }
00472     }
00473 }
00474 
00475 void ompl::geometric::BallTreeRRTstar::getPlannerData(base::PlannerData &data) const
00476 {
00477     Planner::getPlannerData(data);
00478 
00479     std::vector<Motion*> motions;
00480     if (nn_)
00481         nn_->list(motions);
00482 
00483     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00484         data.addEdge (base::PlannerDataVertex (motions[i]->parent ? motions[i]->parent->state : NULL),
00485                       base::PlannerDataVertex (motions[i]->state));
00486 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines