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