All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
BasicPRM.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, 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 /* Author: Ioan Sucan */
00036 
00037 #include "ompl/geometric/planners/prm/BasicPRM.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
00040 #include <algorithm>
00041 #include <queue>
00042 #include <limits>
00043 
00047 static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK = 2;
00048 
00049 
00050 void ompl::geometric::BasicPRM::setup(void)
00051 {
00052     Planner::setup();
00053     if (!nn_)
00054         nn_.reset(new NearestNeighborsSqrtApprox<Milestone*>());
00055     nn_->setDistanceFunction(boost::bind(&BasicPRM::distanceFunction, this, _1, _2));
00056 }
00057 
00058 void ompl::geometric::BasicPRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
00059 {
00060     Planner::setProblemDefinition(pdef);
00061     startM_.clear();
00062     goalM_.clear();
00063 }
00064 
00065 void ompl::geometric::BasicPRM::clear(void)
00066 {
00067     Planner::clear();
00068     sampler_.reset();
00069     freeMemory();
00070     if (nn_)
00071         nn_->clear();
00072     milestones_.clear();
00073     startM_.clear();
00074     goalM_.clear();
00075     componentCount_ = 0;
00076     componentSizes_.clear();
00077     lastStart_ = NULL;
00078     lastGoal_ = NULL;
00079 }
00080 
00081 void ompl::geometric::BasicPRM::freeMemory(void)
00082 {
00083     for (unsigned int i = 0 ; i < milestones_.size() ; ++i)
00084     {
00085         if (milestones_[i]->state)
00086             si_->freeState(milestones_[i]->state);
00087         delete milestones_[i];
00088     }
00089 }
00090 
00091 void ompl::geometric::BasicPRM::growRoadmap(double growTime)
00092 {
00093     time::point endTime = time::now() + time::seconds(growTime);
00094     base::State *workState = si_->allocState();
00095     while (time::now() < endTime)
00096     {
00097         // search for a valid state
00098         bool found = false;
00099         while (!found && time::now() < endTime)
00100         {
00101             unsigned int attempts = 0;
00102             do
00103             {
00104                 found = sampler_->sample(workState);
00105                 attempts++;
00106             } while (attempts < FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
00107         }
00108         // add it as a milestone
00109         if (found)
00110             addMilestone(si_->cloneState(workState));
00111     }
00112     si_->freeState(workState);
00113 }
00114 
00115 void ompl::geometric::BasicPRM::growRoadmap(const std::vector<Milestone*> &start,
00116                                             const std::vector<Milestone*> &goal,
00117                                             const base::PlannerTerminationCondition &ptc,
00118                                             base::State *workState)
00119 {
00120     while (ptc() == false)
00121     {
00122         // search for a valid state
00123         bool found = false;
00124         while (!found && ptc() == false)
00125         {
00126             unsigned int attempts = 0;
00127             do
00128             {
00129                 found = sampler_->sample(workState);
00130                 attempts++;
00131             } while (attempts < FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
00132         }
00133         // add it as a milestone
00134         if (found)
00135         {
00136             addMilestone(si_->cloneState(workState));
00137             if (haveSolution(start, goal))
00138                 break;
00139         }
00140     }
00141 }
00142 
00143 bool ompl::geometric::BasicPRM::haveSolution(const std::vector<Milestone*> &start, const std::vector<Milestone*> &goal,
00144                                              std::pair<Milestone*, Milestone*> *endpoints)
00145 {
00146     base::Goal *g = pdef_->getGoal().get();
00147     for (unsigned int i = 0 ; i < goal.size() ; ++i)
00148         for (unsigned int j = 0 ; j < start.size() ; ++j)
00149             if (goal[i]->component == start[j]->component && g->isStartGoalPairValid(goal[i]->state, start[j]->state))
00150             {
00151                 if (endpoints)
00152                 {
00153                     endpoints->first = start[j];
00154                     endpoints->second = goal[i];
00155                 }
00156                 return true;
00157             }
00158     return false;
00159 }
00160 
00161 namespace ompl
00162 {
00163     // we grow a roadmap until the planner needs to stop or until a maximum amount of time has been reached
00164     static bool growRoadmapTerminationCondition(const base::PlannerTerminationCondition &ptc, const time::point &endTime)
00165     {
00166         return ptc() || time::now() >= endTime;
00167     }
00168 }
00169 
00170 bool ompl::geometric::BasicPRM::solve(const base::PlannerTerminationCondition &ptc)
00171 {
00172     checkValidity();
00173     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00174 
00175     if (!goal)
00176     {
00177         msg_.error("Goal undefined or unknown type of goal");
00178         return false;
00179     }
00180 
00181     unsigned int nrStartStates = milestones_.size();
00182 
00183     // add the valid start states as milestones
00184     while (const base::State *st = pis_.nextStart())
00185         startM_.push_back(addMilestone(si_->cloneState(st)));
00186 
00187     if (startM_.size() == 0)
00188     {
00189         msg_.error("There are no valid initial states!");
00190         return false;
00191     }
00192 
00193     if (!goal->canSample())
00194     {
00195         msg_.error("Insufficient states in sampleable goal region");
00196         return false;
00197     }
00198 
00199     if (!sampler_)
00200         sampler_ = si_->allocValidStateSampler();
00201 
00202     msg_.inform("Starting with %u states", nrStartStates);
00203 
00204     base::State *xstate = si_->allocState();
00205     std::pair<Milestone*, Milestone*> solEndpoints;
00206 
00207     while (ptc() == false)
00208     {
00209         // find at least one valid goal state
00210         if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
00211         {
00212             const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
00213             if (st)
00214                 goalM_.push_back(addMilestone(si_->cloneState(st)));
00215             if (goalM_.empty())
00216             {
00217                 msg_.error("Unable to find any valid goal states");
00218                 break;
00219             }
00220         }
00221 
00222         // if there already is a solution, construct it
00223         if (haveSolution(startM_, goalM_, &solEndpoints))
00224         {
00225             constructSolution(solEndpoints.first, solEndpoints.second);
00226             break;
00227         }
00228         // othewise, spend some time building a roadmap
00229         else
00230         {
00231             // if it is worth looking at other goal regions, plan for part of the time
00232             if (goal->maxSampleCount() > goalM_.size())
00233                 growRoadmap(startM_, goalM_, boost::bind(&growRoadmapTerminationCondition, ptc, time::now() + time::seconds(0.1)), xstate);
00234             // otherwise, just go ahead and build the roadmap
00235             else
00236                 growRoadmap(startM_, goalM_, ptc, xstate);
00237             // if a solution has been found, construct it
00238             if (haveSolution(startM_, goalM_, &solEndpoints))
00239             {
00240                 constructSolution(solEndpoints.first, solEndpoints.second);
00241                 break;
00242             }
00243         }
00244     }
00245     si_->freeState(xstate);
00246 
00247     msg_.inform("Created %u states", milestones_.size() - nrStartStates);
00248 
00249     return goal->isAchieved();
00250 }
00251 
00252 void ompl::geometric::BasicPRM::nearestNeighbors(Milestone *milestone, std::vector<Milestone*> &nbh)
00253 {
00254     nn_->nearestK(milestone, maxNearestNeighbors_, nbh);
00255 }
00256 
00257 ompl::geometric::BasicPRM::Milestone* ompl::geometric::BasicPRM::addMilestone(base::State *state)
00258 {
00259     Milestone *m = new Milestone();
00260     m->state = state;
00261     m->component = componentCount_;
00262     componentSizes_[m->component] = 1;
00263 
00264     // connect to nearest neighbors
00265     std::vector<Milestone*> nbh;
00266     nearestNeighbors(m, nbh);
00267 
00268     for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00269         if (si_->checkMotion(m->state, nbh[i]->state))
00270         {
00271             m->adjacent.push_back(nbh[i]);
00272             nbh[i]->adjacent.push_back(m);
00273             m->costs.push_back(si_->distance(m->state, nbh[i]->state));
00274             nbh[i]->costs.push_back(m->costs.back());
00275             uniteComponents(m, nbh[i]);
00276         }
00277 
00278     // if the new milestone was not absorbed in an existing component,
00279     // increase the number of components
00280     if (m->component == componentCount_)
00281         componentCount_++;
00282     m->index = milestones_.size();
00283     milestones_.push_back(m);
00284     nn_->add(m);
00285     return m;
00286 }
00287 
00288 void ompl::geometric::BasicPRM::uniteComponents(Milestone *m1, Milestone *m2)
00289 {
00290     if (m1->component == m2->component)
00291         return;
00292 
00293     if (componentSizes_[m1->component] > componentSizes_[m2->component])
00294         std::swap(m1, m2);
00295 
00296     const unsigned long c = m2->component;
00297     componentSizes_[c] += componentSizes_[m1->component];
00298     componentSizes_.erase(m1->component);
00299 
00300     std::queue<Milestone*> q;
00301     q.push(m1);
00302 
00303     while (!q.empty())
00304     {
00305         Milestone *m = q.front();
00306         q.pop();
00307         if (m->component != c)
00308         {
00309             m->component = c;
00310             for (unsigned int i = 0 ; i < m->adjacent.size() ; ++i)
00311                 if (m->adjacent[i]->component != c)
00312                     q.push(m->adjacent[i]);
00313         }
00314     }
00315 }
00316 
00317 void ompl::geometric::BasicPRM::reconstructLastSolution(void)
00318 {
00319     if (lastStart_ && lastGoal_)
00320         constructSolution(lastStart_, lastGoal_);
00321     else
00322         msg_.warn("There is no solution to reconstruct");
00323 }
00324 
00325 void ompl::geometric::BasicPRM::constructSolution(const Milestone* start, const Milestone* goal)
00326 {
00327     const unsigned int N = milestones_.size();
00328     std::vector<double> dist(N, std::numeric_limits<double>::infinity());
00329     std::vector<int>    prev(N, -1);
00330     std::vector<int>    seen(N, 0);
00331 
00332     dist[goal->index] = 0.0;
00333     for (unsigned int i = 0 ; i < N ; ++i)
00334     {
00335         double minDist = std::numeric_limits<double>::infinity();
00336         int index = -1;
00337         for (unsigned int j = 0 ; j < N ; ++j)
00338             if (seen[j] == 0 && dist[j] < minDist)
00339             {
00340                 minDist = dist[j];
00341                 index = j;
00342             }
00343         if (index < 0)
00344             break;
00345         seen[index] = 1;
00346 
00347         for (unsigned int j = 0 ; j < milestones_[index]->adjacent.size() ; ++j)
00348         {
00349             const unsigned int idx = milestones_[index]->adjacent[j]->index;
00350             double altDist = dist[index] + milestones_[index]->costs[j];
00351             if (altDist < dist[idx])
00352             {
00353                 dist[idx] = altDist;
00354                 prev[idx] = index;
00355             }
00356         }
00357     }
00358     if (prev[start->index] >= 0)
00359     {
00360         PathGeometric *p = new PathGeometric(si_);
00361         int pos = start->index;
00362         do
00363         {
00364             p->states.push_back(si_->cloneState(milestones_[pos]->state));
00365             pos = prev[pos];
00366         } while (pos >= 0);
00367         pdef_->getGoal()->setSolutionPath(base::PathPtr(p));
00368         lastStart_ = start;
00369         lastGoal_ = goal;
00370     }
00371     else
00372         throw Exception(name_, "Internal error in computing shortest path");
00373 }
00374 
00375 void ompl::geometric::BasicPRM::getPlannerData(base::PlannerData &data) const
00376 {
00377     Planner::getPlannerData(data);
00378 
00379     for (unsigned int i = 0 ; i < milestones_.size() ; ++i)
00380         for (unsigned int j = 0 ; j < milestones_[i]->adjacent.size() ; ++j)
00381         {
00382             data.recordEdge(milestones_[i]->state, milestones_[i]->adjacent[j]->state);
00383             data.tagState(milestones_[i]->state, milestones_[i]->component);
00384         }
00385 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator