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