All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
PRM.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 /* Author: Ioan Sucan, James D. Marble */
00036 
00037 #include "ompl/geometric/planners/prm/PRM.h"
00038 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00041 #include "ompl/datastructures/PDF.h"
00042 #include <boost/lambda/bind.hpp>
00043 #include <boost/graph/astar_search.hpp>
00044 #include <boost/graph/incremental_components.hpp>
00045 #include <boost/property_map/vector_property_map.hpp>
00046 #include <boost/foreach.hpp>
00047 
00048 #define foreach BOOST_FOREACH
00049 #define foreach_reverse BOOST_REVERSE_FOREACH
00050 
00051 namespace ompl
00052 {
00053     namespace magic
00054     {
00055 
00059         static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK = 2;
00060 
00063         static const unsigned int MAX_RANDOM_BOUNCE_STEPS   = 5;
00064 
00067         static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
00068     }
00069 }
00070 
00071 ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy) :
00072     base::Planner(si, "PRM"),
00073     starStrategy_(starStrategy),
00074     stateProperty_(boost::get(vertex_state_t(), g_)),
00075     totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_)),
00076     successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_)),
00077     weightProperty_(boost::get(boost::edge_weight, g_)),
00078     edgeIDProperty_(boost::get(boost::edge_index, g_)),
00079     disjointSets_(boost::get(boost::vertex_rank, g_),
00080                   boost::get(boost::vertex_predecessor, g_)),
00081     maxEdgeID_(0),
00082     userSetConnectionStrategy_(false)
00083 {
00084     specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00085     specs_.approximateSolutions = true;
00086     specs_.optimizingPaths = true;
00087 
00088     Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors);
00089 }
00090 
00091 ompl::geometric::PRM::~PRM(void)
00092 {
00093     freeMemory();
00094 }
00095 
00096 void ompl::geometric::PRM::setup(void)
00097 {
00098     Planner::setup();
00099     if (!nn_)
00100         nn_.reset(new NearestNeighborsGNAT<Vertex>());
00101     nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
00102     if (!connectionStrategy_)
00103     {
00104         if (starStrategy_)
00105             connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension());
00106         else
00107             connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
00108     }
00109     if (!connectionFilter_)
00110         connectionFilter_ = boost::lambda::constant(true);
00111 }
00112 
00113 void ompl::geometric::PRM::setMaxNearestNeighbors(unsigned int k)
00114 {
00115     if (!setup_)
00116         setup();
00117     connectionStrategy_ = KStrategy<Vertex>(k, nn_);
00118 }
00119 
00120 void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
00121 {
00122     Planner::setProblemDefinition(pdef);
00123     startM_.clear();
00124     goalM_.clear();
00125 }
00126 
00127 void ompl::geometric::PRM::clear(void)
00128 {
00129     Planner::clear();
00130     sampler_.reset();
00131     simpleSampler_.reset();
00132     freeMemory();
00133     if (nn_)
00134         nn_->clear();
00135     startM_.clear();
00136     goalM_.clear();
00137     maxEdgeID_ = 0;
00138 }
00139 
00140 void ompl::geometric::PRM::freeMemory(void)
00141 {
00142     foreach (Vertex v, boost::vertices(g_))
00143         si_->freeState(stateProperty_[v]);
00144     g_.clear();
00145 }
00146 
00147 void ompl::geometric::PRM::expandRoadmap(double expandTime)
00148 {
00149     if (!simpleSampler_)
00150         simpleSampler_ = si_->allocStateSampler();
00151 
00152     std::vector<Vertex> empty;
00153     std::vector<base::State*> states(magic::MAX_RANDOM_BOUNCE_STEPS);
00154     si_->allocStates(states);
00155     expandRoadmap(empty, empty, base::timedPlannerTerminationCondition(expandTime), states);
00156     si_->freeStates(states);
00157 }
00158 
00159 void ompl::geometric::PRM::expandRoadmap(const std::vector<Vertex> &starts,
00160                                          const std::vector<Vertex> &goals,
00161                                          const base::PlannerTerminationCondition &ptc,
00162                                          std::vector<base::State*> &workStates)
00163 {
00164     // construct a probability distribution over the vertices in the roadmap
00165     // as indicated in
00166     //  "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
00167     //        Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
00168 
00169     PDF<Vertex> pdf;
00170     foreach (Vertex v, boost::vertices(g_))
00171     {
00172         const unsigned int t = totalConnectionAttemptsProperty_[v];
00173         pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
00174     }
00175 
00176     if (pdf.empty())
00177         return;
00178 
00179     while (ptc() == false)
00180     {
00181         Vertex v = pdf.sample(rng_.uniform01());
00182         unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
00183         if (s > 0)
00184         {
00185             s--;
00186             Vertex last = addMilestone(si_->cloneState(workStates[s]));
00187 
00188             for (unsigned int i = 0 ; i < s ; ++i)
00189             {
00190                 // add the vertex along the bouncing motion
00191                 Vertex m = boost::add_vertex(g_);
00192                 stateProperty_[m] = si_->cloneState(workStates[i]);
00193                 totalConnectionAttemptsProperty_[m] = 1;
00194                 successfulConnectionAttemptsProperty_[m] = 0;
00195                 disjointSets_.make_set(m);
00196 
00197                 // add the edge to the parent vertex
00198                 const double weight = distanceFunction(v, m);
00199                 const unsigned int id = maxEdgeID_++;
00200                 const Graph::edge_property_type properties(weight, id);
00201                 boost::add_edge(v, m, properties, g_);
00202                 uniteComponents(v, m);
00203 
00204                 // add the vertext to the nearest neighbors data structure
00205                 nn_->add(m);
00206                 v = m;
00207             }
00208 
00209             // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
00210             // we add an edge
00211             if (s > 0 || !boost::same_component(v, last, disjointSets_))
00212             {
00213                 // add the edge to the parent vertex
00214                 const double weight = distanceFunction(v, last);
00215                 const unsigned int id = maxEdgeID_++;
00216                 const Graph::edge_property_type properties(weight, id);
00217                 boost::add_edge(v, last, properties, g_);
00218                 uniteComponents(v, last);
00219             }
00220             if (haveSolution(starts, goals))
00221                 break;
00222         }
00223     }
00224 }
00225 
00226 void ompl::geometric::PRM::growRoadmap(double growTime)
00227 {
00228     if (!isSetup())
00229         setup();
00230     if (!sampler_)
00231         sampler_ = si_->allocValidStateSampler();
00232 
00233     time::point endTime = time::now() + time::seconds(growTime);
00234     base::State *workState = si_->allocState();
00235     while (time::now() < endTime)
00236     {
00237         // search for a valid state
00238         bool found = false;
00239         while (!found && time::now() < endTime)
00240         {
00241             unsigned int attempts = 0;
00242             do
00243             {
00244                 found = sampler_->sample(workState);
00245                 attempts++;
00246             } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
00247         }
00248         // add it as a milestone
00249         if (found)
00250             addMilestone(si_->cloneState(workState));
00251     }
00252     si_->freeState(workState);
00253 }
00254 
00255 void ompl::geometric::PRM::growRoadmap(const std::vector<Vertex> &start,
00256                                        const std::vector<Vertex> &goal,
00257                                        const base::PlannerTerminationCondition &ptc,
00258                                        base::State *workState)
00259 {
00260     while (ptc() == false)
00261     {
00262         // search for a valid state
00263         bool found = false;
00264         while (!found && ptc() == false)
00265         {
00266             unsigned int attempts = 0;
00267             do
00268             {
00269                 found = sampler_->sample(workState);
00270                 attempts++;
00271             } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
00272         }
00273         // add it as a milestone
00274         if (found)
00275         {
00276             addMilestone(si_->cloneState(workState));
00277             if (haveSolution(start, goal))
00278                 break;
00279         }
00280     }
00281 }
00282 
00283 bool ompl::geometric::PRM::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, std::pair<Vertex, Vertex> *endpoints)
00284 {
00285     base::Goal *g = pdef_->getGoal().get();
00286     foreach (Vertex start, starts)
00287         foreach (Vertex goal, goals)
00288         {
00289 
00290             if (boost::same_component(start, goal, disjointSets_) &&
00291                 g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
00292             {
00293                 bool sol = true;
00294 
00295                 // if there is a maximum path length we wish to accept, we need to check the solution length
00296                 if (g->getMaximumPathLength() < std::numeric_limits<double>::infinity())
00297                 {
00298                     base::PathPtr p = constructSolution(start, goal);
00299                     double pl = p->length();
00300                     if (pl > g->getMaximumPathLength())
00301                     {
00302                         sol = false;
00303                         // record approximate solution
00304                         if (approxlen_ < 0.0 || approxlen_ > pl)
00305                         {
00306                             approxsol_ = p;
00307                             approxlen_ = pl;
00308                         }
00309                     }
00310                 }
00311 
00312                 if (sol)
00313                 {
00314                     if (endpoints)
00315                     {
00316                         endpoints->first = start;
00317                         endpoints->second = goal;
00318                     }
00319                     return true;
00320                 }
00321             }
00322         }
00323     return false;
00324 }
00325 
00326 bool ompl::geometric::PRM::solve(const base::PlannerTerminationCondition &ptc)
00327 {
00328     checkValidity();
00329     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00330 
00331     if (!goal)
00332     {
00333         msg_.error("Goal undefined or unknown type of goal");
00334         return false;
00335     }
00336 
00337     unsigned int nrStartStates = boost::num_vertices(g_);
00338 
00339     // add the valid start states as milestones
00340     while (const base::State *st = pis_.nextStart())
00341         startM_.push_back(addMilestone(si_->cloneState(st)));
00342 
00343     if (startM_.size() == 0)
00344     {
00345         msg_.error("There are no valid initial states!");
00346         return false;
00347     }
00348 
00349     if (!goal->couldSample())
00350     {
00351         msg_.error("Insufficient states in sampleable goal region");
00352         return false;
00353     }
00354 
00355     if (!sampler_)
00356         sampler_ = si_->allocValidStateSampler();
00357     if (!simpleSampler_)
00358         simpleSampler_ = si_->allocStateSampler();
00359 
00360     approxsol_.reset();
00361     approxlen_ = -1.0;
00362 
00363     msg_.inform("Starting with %u states", nrStartStates);
00364 
00365     std::vector<base::State*> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
00366     si_->allocStates(xstates);
00367     std::pair<Vertex, Vertex> solEndpoints;
00368     unsigned int steps = 0;
00369     bool addedSolution = false;
00370 
00371     while (ptc() == false)
00372     {
00373         // find at least one valid goal state
00374         if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
00375         {
00376             const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
00377             if (st)
00378                 goalM_.push_back(addMilestone(si_->cloneState(st)));
00379             if (goalM_.empty())
00380             {
00381                 msg_.error("Unable to find any valid goal states");
00382                 break;
00383             }
00384         }
00385 
00386         // if there already is a solution, construct it
00387         if (haveSolution(startM_, goalM_, &solEndpoints))
00388         {
00389             goal->addSolutionPath(constructSolution(solEndpoints.first, solEndpoints.second));
00390             addedSolution = true;
00391             break;
00392         }
00393         // othewise, spend some time building a roadmap
00394         else
00395         {
00396             // maintain a 2:1 ration for growing:expansion of roadmap
00397             // call growRoadmap() 4 times for 0.1 seconds, for every call of expandRoadmap() for 0.2 seconds
00398             if (steps < 4)
00399             {
00400                 growRoadmap(startM_, goalM_, base::PlannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(0.1)), xstates[0]);
00401                 steps++;
00402             }
00403             else
00404             {
00405                 expandRoadmap(startM_, goalM_, base::PlannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(0.2)), xstates);
00406                 steps = 0;
00407             }
00408 
00409             // if a solution has been found, construct it
00410             if (haveSolution(startM_, goalM_, &solEndpoints))
00411             {
00412                 goal->addSolutionPath(constructSolution(solEndpoints.first, solEndpoints.second));
00413                 addedSolution = true;
00414                 break;
00415             }
00416         }
00417     }
00418     si_->freeStates(xstates);
00419 
00420     msg_.inform("Created %u states", boost::num_vertices(g_) - nrStartStates);
00421 
00422     if (!addedSolution && approxsol_)
00423     {
00424         // the solution is exact, but not as short as we'd like it to be
00425         goal->addSolutionPath(approxsol_, true, 0.0);
00426         addedSolution = true;
00427     }
00428 
00429     return addedSolution;
00430 }
00431 
00432 ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state)
00433 {
00434     Vertex m = boost::add_vertex(g_);
00435     stateProperty_[m] = state;
00436     totalConnectionAttemptsProperty_[m] = 1;
00437     successfulConnectionAttemptsProperty_[m] = 0;
00438 
00439     // Initialize to its own (dis)connected component.
00440     disjointSets_.make_set(m);
00441 
00442     // Which milestones will we attempt to connect to?
00443     if (!connectionStrategy_)
00444         throw Exception(name_, "No connection strategy!");
00445 
00446     const std::vector<Vertex>& neighbors = connectionStrategy_(m);
00447 
00448     foreach (Vertex n, neighbors)
00449         if ((boost::same_component(m, n, disjointSets_) || connectionFilter_(m, n)))
00450         {
00451             totalConnectionAttemptsProperty_[m]++;
00452             totalConnectionAttemptsProperty_[n]++;
00453             if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
00454             {
00455                 successfulConnectionAttemptsProperty_[m]++;
00456                 successfulConnectionAttemptsProperty_[n]++;
00457                 const double weight = distanceFunction(m, n);
00458                 const unsigned int id = maxEdgeID_++;
00459                 const Graph::edge_property_type properties(weight, id);
00460                 boost::add_edge(m, n, properties, g_);
00461                 uniteComponents(n, m);
00462             }
00463         }
00464 
00465     nn_->add(m);
00466     return m;
00467 }
00468 
00469 void ompl::geometric::PRM::uniteComponents(Vertex m1, Vertex m2)
00470 {
00471     disjointSets_.union_set(m1, m2);
00472 }
00473 
00474 ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex start, const Vertex goal)
00475 {
00476     PathGeometric *p = new PathGeometric(si_);
00477 
00478     boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
00479 
00480     boost::astar_search(g_, start,
00481             boost::bind(&PRM::distanceFunction, this, _1, goal),
00482             boost::predecessor_map(prev));
00483 
00484     if (prev[goal] == goal)
00485         throw Exception(name_, "Could not find solution path");
00486     else
00487         for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
00488             p->append(stateProperty_[pos]);
00489     p->append(stateProperty_[start]);
00490     p->reverse();
00491 
00492     return base::PathPtr(p);
00493 }
00494 
00495 void ompl::geometric::PRM::getPlannerData(base::PlannerData &data) const
00496 {
00497     Planner::getPlannerData(data);
00498 
00499     foreach(const Edge e, boost::edges(g_))
00500     {
00501         const Vertex v1 = boost::source(e, g_);
00502         const Vertex v2 = boost::target(e, g_);
00503         data.recordEdge(stateProperty_[v1], stateProperty_[v2]);
00504     }
00505 }