ompl/geometric/planners/prm/src/SPARS.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
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 Rutgers 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: Andrew Dobson */
00036 
00037 #include "ompl/geometric/planners/prm/SPARS.h"
00038 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
00039 #include "ompl/base/goals/GoalSampleableRegion.h"
00040 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00041 #include "ompl/tools/config/SelfConfig.h"
00042 #include "ompl/tools/config/MagicConstants.h"
00043 #include <boost/bind.hpp>
00044 #include <boost/graph/astar_search.hpp>
00045 #include <boost/graph/incremental_components.hpp>
00046 #include <boost/property_map/vector_property_map.hpp>
00047 #include <boost/foreach.hpp>
00048 
00049 #include "GoalVisitor.hpp"
00050 
00051 #define foreach BOOST_FOREACH
00052 #define foreach_reverse BOOST_REVERSE_FOREACH
00053 
00054 ompl::geometric::SPARS::SPARS(const base::SpaceInformationPtr &si) :
00055     base::Planner(si, "SPARS"),
00056     geomPath_(si),
00057     stateProperty_(boost::get(vertex_state_t(), g_)),
00058     sparseStateProperty_(boost::get(vertex_state_t(), s_)),
00059     sparseColorProperty_(boost::get(vertex_color_t(), s_)),
00060     representativesProperty_(boost::get(vertex_representative_t(), g_)),
00061     nonInterfaceListsProperty_(boost::get(vertex_list_t(), s_)),
00062     interfaceListsProperty_(boost::get(vertex_interface_list_t(), s_)),
00063     weightProperty_(boost::get(boost::edge_weight, g_)),
00064     sparseDJSets_(boost::get(boost::vertex_rank, s_),
00065                   boost::get(boost::vertex_predecessor, s_)),
00066     consecutiveFailures_(0),
00067     stretchFactor_(3.),
00068     maxFailures_(1000),
00069     addedSolution_(false),
00070     denseDeltaFraction_(.001),
00071     sparseDeltaFraction_(.25),
00072     denseDelta_(0.),
00073     sparseDelta_(0.),
00074     iterations_(0),
00075     bestCost_(std::numeric_limits<double>::quiet_NaN())
00076 {
00077     specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00078     specs_.approximateSolutions = false;
00079     specs_.optimizingPaths = true;
00080     specs_.multithreaded = true;
00081 
00082     psimp_.reset(new PathSimplifier(si_));
00083     psimp_->freeStates(false);
00084 
00085     Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:3.0");
00086     Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction, &SPARS::getSparseDeltaFraction, "0.0:0.01:1.0");
00087     Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction, &SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1");
00088     Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:3000");
00089 
00090     addPlannerProgressProperty("iterations INTEGER",
00091                                boost::bind(&SPARS::getIterationCount, this));
00092     addPlannerProgressProperty("best cost REAL",
00093                                boost::bind(&SPARS::getBestCost, this));
00094 }
00095 
00096 ompl::geometric::SPARS::~SPARS()
00097 {
00098     freeMemory();
00099 }
00100 
00101 void ompl::geometric::SPARS::setup()
00102 {
00103     Planner::setup();
00104     if (!nn_)
00105         nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(si_->getStateSpace()));
00106     nn_->setDistanceFunction(boost::bind(&SPARS::distanceFunction, this, _1, _2));
00107     if (!snn_)
00108         snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(si_->getStateSpace()));
00109     snn_->setDistanceFunction(boost::bind(&SPARS::sparseDistanceFunction, this, _1, _2));
00110     if (!connectionStrategy_)
00111         connectionStrategy_ = KStarStrategy<DenseVertex>(boost::bind(&SPARS::milestoneCount, this), nn_, si_->getStateDimension());
00112     double maxExt = si_->getMaximumExtent();
00113     sparseDelta_ = sparseDeltaFraction_ * maxExt;
00114     denseDelta_ = denseDeltaFraction_ * maxExt;
00115 
00116     // Setup optimization objective
00117     //
00118     // If no optimization objective was specified, then default to
00119     // optimizing path length as computed by the distance() function
00120     // in the state space.
00121     if (pdef_)
00122     {
00123         if (pdef_->hasOptimizationObjective())
00124         {
00125             opt_ = pdef_->getOptimizationObjective();
00126             if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt_.get()))
00127                 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str());
00128         }
00129         else
00130             opt_.reset(new base::PathLengthOptimizationObjective(si_));
00131     }
00132     else
00133     {
00134         OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
00135         setup_ = false;
00136     }
00137 }
00138 
00139 void ompl::geometric::SPARS::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
00140 {
00141     Planner::setProblemDefinition(pdef);
00142     clearQuery();
00143 }
00144 
00145 void ompl::geometric::SPARS::resetFailures()
00146 {
00147     consecutiveFailures_ = 0;
00148 }
00149 
00150 void ompl::geometric::SPARS::clearQuery()
00151 {
00152     startM_.clear();
00153     goalM_.clear();
00154     pis_.restart();
00155 }
00156 
00157 void ompl::geometric::SPARS::clear()
00158 {
00159     Planner::clear();
00160     sampler_.reset();
00161     simpleSampler_.reset();
00162     freeMemory();
00163     if (nn_)
00164         nn_->clear();
00165     if (snn_)
00166         snn_->clear();
00167     clearQuery();
00168     resetFailures();
00169     iterations_ = 0;
00170     bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
00171 }
00172 
00173 void ompl::geometric::SPARS::freeMemory()
00174 {
00175     foreach (DenseVertex v, boost::vertices(g_))
00176         if( stateProperty_[v] != NULL )
00177         {
00178             si_->freeState(stateProperty_[v]);
00179             stateProperty_[v] = NULL;
00180         }
00181     foreach (SparseVertex n, boost::vertices(s_))
00182         if( sparseStateProperty_[n] != NULL )
00183         {
00184             si_->freeState(sparseStateProperty_[n]);
00185             sparseStateProperty_[n] = NULL;
00186         }
00187     s_.clear();
00188     g_.clear();
00189 }
00190 
00191 ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::addSample(base::State *workState, const base::PlannerTerminationCondition &ptc)
00192 {
00193     DenseVertex result = boost::graph_traits<DenseGraph>::null_vertex();
00194 
00195     // search for a valid state
00196     bool found = false;
00197     while (!found && ptc == false)
00198     {
00199         unsigned int attempts = 0;
00200         do
00201         {
00202             found = sampler_->sample(workState);
00203             attempts++;
00204         } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
00205     }
00206 
00207     if (found)
00208         result = addMilestone(si_->cloneState(workState));
00209     return result;
00210 }
00211 
00212 void ompl::geometric::SPARS::checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
00213 {
00214     base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00215     while (!ptc && !addedSolution_)
00216     {
00217         // Check for any new goal states
00218         if (goal->maxSampleCount() > goalM_.size())
00219         {
00220             const base::State *st = pis_.nextGoal();
00221             if (st)
00222             {
00223                 addMilestone(si_->cloneState(st));
00224                 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
00225             }
00226         }
00227 
00228         // Check for a solution
00229         addedSolution_ = haveSolution(startM_, goalM_, solution);
00230         // Sleep for 1ms
00231         if (!addedSolution_)
00232             boost::this_thread::sleep(boost::posix_time::milliseconds(1));
00233     }
00234 }
00235 
00236 bool ompl::geometric::SPARS::haveSolution(const std::vector<DenseVertex> &starts, const std::vector<DenseVertex> &goals, base::PathPtr &solution)
00237 {
00238     base::Goal *g = pdef_->getGoal().get();
00239     base::Cost sol_cost(opt_->infiniteCost());
00240     foreach (DenseVertex start, starts)
00241     {
00242         foreach (DenseVertex goal, goals)
00243         {
00244             // we lock because the connected components algorithm is incremental and may change disjointSets_
00245             graphMutex_.lock();
00246             bool same_component = sameComponent(start, goal);
00247             graphMutex_.unlock();
00248 
00249             if (same_component && g->isStartGoalPairValid(sparseStateProperty_[goal], sparseStateProperty_[start]))
00250             {
00251                 base::PathPtr p = constructSolution(start, goal);
00252                 if (p)
00253                 {
00254                     base::Cost pathCost = p->cost(opt_);
00255                     if (opt_->isCostBetterThan(pathCost, bestCost_))
00256                         bestCost_ = pathCost;
00257                     // Check if optimization objective is satisfied
00258                     if (opt_->isSatisfied(pathCost))
00259                     {
00260                         solution = p;
00261                         return true;
00262                     }
00263                     else if (opt_->isCostBetterThan(pathCost, sol_cost))
00264                     {
00265                         solution = p;
00266                         sol_cost = pathCost;
00267                     }
00268                 }
00269             }
00270         }
00271     }
00272 
00273     return false;
00274 }
00275 
00276 bool ompl::geometric::SPARS::reachedTerminationCriterion() const
00277 {
00278     return consecutiveFailures_ >= maxFailures_ || addedSolution_;
00279 }
00280 
00281 bool ompl::geometric::SPARS::reachedFailureLimit() const
00282 {
00283     return consecutiveFailures_ >= maxFailures_;
00284 }
00285 
00286 void ompl::geometric::SPARS::checkQueryStateInitialization()
00287 {
00288     boost::mutex::scoped_lock _(graphMutex_);
00289     if (boost::num_vertices(g_) < 1)
00290     {
00291         sparseQueryVertex_ = boost::add_vertex(s_);
00292         queryVertex_ = boost::add_vertex(g_);
00293         sparseStateProperty_[sparseQueryVertex_] = NULL;
00294         stateProperty_[queryVertex_] = NULL;
00295     }
00296 }
00297 
00298 bool ompl::geometric::SPARS::sameComponent(SparseVertex m1, SparseVertex m2)
00299 {
00300     return boost::same_component(m1, m2, sparseDJSets_);
00301 }
00302 
00303 ompl::base::PlannerStatus ompl::geometric::SPARS::solve(const base::PlannerTerminationCondition &ptc)
00304 {
00305     checkValidity();
00306     checkQueryStateInitialization();
00307 
00308     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00309 
00310     if (!goal)
00311     {
00312         OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
00313         return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
00314     }
00315 
00316     // Add the valid start states as milestones
00317     while (const base::State *st = pis_.nextStart())
00318     {
00319         addMilestone(si_->cloneState(st));
00320         startM_.push_back(addGuard(si_->cloneState(st), START ));
00321     }
00322     if (startM_.empty())
00323     {
00324         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00325         return base::PlannerStatus::INVALID_START;
00326     }
00327 
00328     if (goalM_.empty() && !goal->couldSample())
00329     {
00330         OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
00331         return base::PlannerStatus::INVALID_GOAL;
00332     }
00333 
00334     // Add the valid goal states as milestones
00335     while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
00336     {
00337         addMilestone(si_->cloneState(st));
00338         goalM_.push_back(addGuard(si_->cloneState(st), GOAL ));
00339     }
00340     if (goalM_.empty())
00341     {
00342         OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
00343         return base::PlannerStatus::INVALID_GOAL;
00344     }
00345 
00346     unsigned int nrStartStatesDense = boost::num_vertices(g_) - 1; // don't count query vertex
00347     unsigned int nrStartStatesSparse = boost::num_vertices(s_) - 1; // don't count query vertex
00348     OMPL_INFORM("%s: Starting planning with %u dense states, %u sparse states", getName().c_str(), nrStartStatesDense, nrStartStatesSparse);
00349 
00350     // Reset addedSolution_ member
00351     addedSolution_ = false;
00352     resetFailures();
00353     base::PathPtr sol;
00354     base::PlannerTerminationCondition ptcOrFail =
00355         base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedFailureLimit, this)));
00356     boost::thread slnThread(boost::bind(&SPARS::checkForSolution, this, ptcOrFail, boost::ref(sol)));
00357 
00358     // Construct planner termination condition which also takes maxFailures_ and addedSolution_ into account
00359     base::PlannerTerminationCondition ptcOrStop =
00360         base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedTerminationCriterion, this)));
00361     constructRoadmap(ptcOrStop);
00362 
00363     // Ensure slnThread is ceased before exiting solve
00364     slnThread.join();
00365 
00366     if (sol)
00367         pdef_->addSolutionPath(sol, false, -1.0, getName());
00368 
00369     OMPL_INFORM("%s: Created %u dense states, %u sparse states", getName().c_str(),
00370                 (unsigned int)(boost::num_vertices(g_) - nrStartStatesDense),
00371                 (unsigned int)(boost::num_vertices(s_) - nrStartStatesSparse));
00372 
00373     // Return true if any solution was found.
00374     return sol ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00375 }
00376 
00377 void ompl::geometric::SPARS::constructRoadmap(const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail)
00378 {
00379     if (stopOnMaxFail)
00380     {
00381         resetFailures();
00382         base::PlannerTerminationCondition ptcOrFail =
00383             base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedFailureLimit, this)));
00384         constructRoadmap(ptcOrFail);
00385     }
00386     else
00387         constructRoadmap(ptc);
00388 }
00389 
00390 void ompl::geometric::SPARS::constructRoadmap(const base::PlannerTerminationCondition &ptc)
00391 {
00392     checkQueryStateInitialization();
00393 
00394     if (!isSetup())
00395         setup();
00396     if (!sampler_)
00397         sampler_ = si_->allocValidStateSampler();
00398     if (!simpleSampler_)
00399         simpleSampler_ = si_->allocStateSampler();
00400 
00401     base::State *workState = si_->allocState();
00402 
00403     /* The whole neighborhood set which has been most recently computed */
00404     std::vector<SparseVertex> graphNeighborhood;
00405 
00406     /* The visible neighborhood set which has been most recently computed */
00407     std::vector<SparseVertex> visibleNeighborhood;
00408 
00409     /* Storage for the interface neighborhood, populated by getInterfaceNeighborhood() */
00410     std::vector<DenseVertex> interfaceNeighborhood;
00411 
00412     bestCost_ = opt_->infiniteCost();
00413     while (ptc == false)
00414     {
00415         iterations_++;
00416 
00417         // Generate a single sample, and attempt to connect it to nearest neighbors.
00418         DenseVertex q = addSample(workState, ptc);
00419         if (q == boost::graph_traits<DenseGraph>::null_vertex())
00420             continue;
00421 
00422         //Now that we've added to D, try adding to S
00423         //Start by figuring out who our neighbors are
00424         getSparseNeighbors(workState, graphNeighborhood);
00425         filterVisibleNeighbors(workState, graphNeighborhood, visibleNeighborhood);
00426         //Check for addition for Coverage
00427         if( !checkAddCoverage(workState, graphNeighborhood))
00428             //If not for Coverage, then Connectivity
00429             if( !checkAddConnectivity(workState, graphNeighborhood))
00430                 //Check for the existence of an interface
00431                 if( !checkAddInterface(graphNeighborhood, visibleNeighborhood, q))
00432                 {
00433                     // Then check to see if it's on an interface
00434                     getInterfaceNeighborhood(q, interfaceNeighborhood);
00435                     if (interfaceNeighborhood.size() > 0)
00436                     {
00437                         //Check for addition for spanner prop
00438                         if (!checkAddPath(q, interfaceNeighborhood))
00439                             //All of the tests have failed.  Report failure for the sample
00440                             ++consecutiveFailures_;
00441                     }
00442                     else
00443                         //There's no interface here, so drop it
00444                         ++consecutiveFailures_;
00445                 }
00446     }
00447 
00448     si_->freeState(workState);
00449 }
00450 
00451 ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::addMilestone(base::State *state)
00452 {
00453     boost::mutex::scoped_lock _(graphMutex_);
00454 
00455     DenseVertex m = boost::add_vertex(g_);
00456     stateProperty_[m] = state;
00457 
00458     // Which milestones will we attempt to connect to?
00459     const std::vector<DenseVertex>& neighbors = connectionStrategy_(m);
00460 
00461     foreach (DenseVertex n, neighbors)
00462         if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
00463         {
00464             const double weight = distanceFunction(m, n);
00465             const DenseGraph::edge_property_type properties(weight);
00466 
00467             boost::add_edge(m, n, properties, g_);
00468         }
00469 
00470     nn_->add(m);
00471 
00472     //Need to update representative information here...
00473     calculateRepresentative(m);
00474 
00475     std::vector<DenseVertex> interfaceNeighborhood;
00476     std::set<SparseVertex> interfaceRepresentatives;
00477 
00478     getInterfaceNeighborRepresentatives(m, interfaceRepresentatives);
00479     getInterfaceNeighborhood(m, interfaceNeighborhood);
00480     addToRepresentatives(m, representativesProperty_[m], interfaceRepresentatives);
00481     foreach (DenseVertex qp, interfaceNeighborhood)
00482     {
00483         removeFromRepresentatives( qp, representativesProperty_[qp] );
00484         getInterfaceNeighborRepresentatives( qp, interfaceRepresentatives );
00485         addToRepresentatives( qp, representativesProperty_[qp], interfaceRepresentatives );
00486     }
00487 
00488     return m;
00489 }
00490 
00491 ompl::geometric::SPARS::SparseVertex ompl::geometric::SPARS::addGuard(base::State *state, GuardType type)
00492 {
00493     boost::mutex::scoped_lock _(graphMutex_);
00494 
00495     SparseVertex v = boost::add_vertex(s_);
00496     sparseStateProperty_[v] = state;
00497     sparseColorProperty_[v] = type;
00498 
00499     sparseDJSets_.make_set(v);
00500 
00501     snn_->add(v);
00502     updateRepresentatives(v);
00503 
00504     resetFailures();
00505     return v;
00506 }
00507 
00508 void ompl::geometric::SPARS::connectSparsePoints(SparseVertex v, SparseVertex vp)
00509 {
00510     const base::Cost weight(costHeuristic(v, vp));
00511     const SpannerGraph::edge_property_type properties(weight);
00512     boost::mutex::scoped_lock _(graphMutex_);
00513     boost::add_edge(v, vp, properties, s_);
00514     sparseDJSets_.union_set(v, vp);
00515 }
00516 
00517 void ompl::geometric::SPARS::connectDensePoints( DenseVertex v, DenseVertex vp )
00518 {
00519     const double weight = distanceFunction(v, vp);
00520     const DenseGraph::edge_property_type properties(weight);
00521     boost::mutex::scoped_lock _(graphMutex_);
00522     boost::add_edge(v, vp, properties, g_);
00523 }
00524 
00525 bool ompl::geometric::SPARS::checkAddCoverage(const base::State *lastState, const std::vector<SparseVertex>& neigh )
00526 {
00527     //For each of these neighbors,
00528     foreach (SparseVertex n, neigh)
00529         //If path between is free
00530         if (si_->checkMotion( lastState, sparseStateProperty_[n]))
00531             //Abort out and return false
00532             return false;
00533     //No free paths means we add for coverage
00534     addGuard(si_->cloneState(lastState), COVERAGE);
00535     return true;
00536 }
00537 
00538 bool ompl::geometric::SPARS::checkAddConnectivity( const base::State *lastState, const std::vector<SparseVertex>& neigh )
00539 {
00540     std::vector< SparseVertex > links;
00541     //For each neighbor
00542     for (std::size_t i = 0; i < neigh.size(); ++i )
00543         //For each other neighbor
00544         for (std::size_t j = i + 1; j < neigh.size(); ++j )
00545             //If they are in different components
00546             if (!sameComponent(neigh[i], neigh[j]))
00547                 //If the paths between are collision free
00548                 if( si_->checkMotion( lastState, sparseStateProperty_[neigh[i]] ) && si_->checkMotion( lastState, sparseStateProperty_[neigh[j]] ) )
00549                 {
00550                     links.push_back( neigh[i] );
00551                     links.push_back( neigh[j] );
00552                 }
00553 
00554     if( links.size() != 0 )
00555     {
00556         //Add the node
00557         SparseVertex g = addGuard( si_->cloneState(lastState), CONNECTIVITY );
00558 
00559         for (std::size_t i = 0; i < links.size(); ++i )
00560             //If there's no edge
00561             if (!boost::edge(g, links[i], s_).second)
00562                 //And the components haven't been united by previous links
00563                 if (!sameComponent(links[i], g))
00564                     connectSparsePoints( g, links[i] );
00565         return true;
00566     }
00567     return false;
00568 }
00569 
00570 bool ompl::geometric::SPARS::checkAddInterface(const std::vector<SparseVertex>& graphNeighborhood, const std::vector<SparseVertex>& visibleNeighborhood, DenseVertex q )
00571 {
00572     //If we have more than 1 neighbor
00573     if( visibleNeighborhood.size() > 1 )
00574         //If our closest neighbors are also visible
00575         if( graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1] )
00576             //If our two closest neighbors don't share an edge
00577             if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], s_).second)
00578             {
00579                 //If they can be directly connected
00580                 if( si_->checkMotion( sparseStateProperty_[visibleNeighborhood[0]], sparseStateProperty_[visibleNeighborhood[1]] ) )
00581                 {
00582                     //Connect them
00583                     connectSparsePoints( visibleNeighborhood[0], visibleNeighborhood[1] );
00584                     //And report that we added to the roadmap
00585                     resetFailures();
00586                     //Report success
00587                     return true;
00588                 }
00589                 else
00590                 {
00591                     //Add the new node to the graph, to bridge the interface
00592                     SparseVertex v = addGuard( si_->cloneState( stateProperty_[q] ), INTERFACE );
00593                     connectSparsePoints( v, visibleNeighborhood[0] );
00594                     connectSparsePoints( v, visibleNeighborhood[1] );
00595                     //Report success
00596                     return true;
00597                 }
00598             }
00599     return false;
00600 }
00601 
00602 bool ompl::geometric::SPARS::checkAddPath(DenseVertex q, const std::vector<DenseVertex>& neigh)
00603 {
00604     bool result = false;
00605 
00606     //Get q's representative => v
00607     SparseVertex v = representativesProperty_[q];
00608 
00609     //Extract the representatives of neigh => n_rep
00610     std::set<SparseVertex> n_rep;
00611     foreach( DenseVertex qp, neigh )
00612         n_rep.insert(representativesProperty_[qp]);
00613 
00614     std::vector<SparseVertex> Xs;
00615     //for each v' in n_rep
00616     for (std::set<SparseVertex>::iterator it = n_rep.begin() ; it != n_rep.end() && !result ; ++it)
00617     {
00618         SparseVertex vp = *it;
00619         //Identify appropriate v" candidates => vpps
00620         std::vector<SparseVertex> VPPs;
00621         computeVPP(v, vp, VPPs);
00622 
00623         foreach( SparseVertex vpp, VPPs )
00624         {
00625             double s_max = 0;
00626             //Find the X nodes to test
00627             computeX(v, vp, vpp, Xs);
00628 
00629             //For each x in xs
00630             foreach( SparseVertex x, Xs )
00631             {
00632                 //Compute/Retain MAXimum distance path thorugh S
00633                 double dist = (si_->distance(sparseStateProperty_[x], sparseStateProperty_[v])
00634                     + si_->distance(sparseStateProperty_[v], sparseStateProperty_[vp])) / 2.0;
00635                 if( dist > s_max )
00636                     s_max = dist;
00637             }
00638 
00639             DensePath bestDPath;
00640             DenseVertex best_qpp = boost::graph_traits<DenseGraph>::null_vertex();
00641             double d_min = std::numeric_limits<double>::infinity(); //Insanely big number
00642             //For each vpp in vpps
00643             for (std::size_t j = 0; j < VPPs.size() && !result; ++j)
00644             {
00645                 SparseVertex vpp = VPPs[j];
00646                 //For each q", which are stored interface nodes on v for i(vpp,v)
00647                 foreach( DenseVertex qpp, interfaceListsProperty_[v].interfaceHash[vpp] )
00648                 {
00649                     // check that representatives are consistent
00650                     assert(representativesProperty_[qpp] == v);
00651 
00652                     //If they happen to be the one and same node
00653                     if (q == qpp)
00654                     {
00655                         bestDPath.push_front( stateProperty_[q] );
00656                         best_qpp = qpp;
00657                         d_min = 0;
00658                     }
00659                     else
00660                     {
00661                         //Compute/Retain MINimum distance path on D through q, q"
00662                         DensePath dPath;
00663                         computeDensePath(q, qpp, dPath);
00664                         if (dPath.size() > 0)
00665                         {
00666                             // compute path length
00667                             double length = 0.0;
00668                             DensePath::const_iterator jt = dPath.begin();
00669                             for (DensePath::const_iterator it = jt + 1 ; it != dPath.end() ; ++it)
00670                             {
00671                                 length += si_->distance(*jt, *it);
00672                                 jt = it;
00673                             }
00674 
00675                             if (length < d_min)
00676                             {
00677                                 d_min = length;
00678                                 bestDPath.swap(dPath);
00679                                 best_qpp = qpp;
00680                             }
00681                         }
00682                     }
00683                 }
00684 
00685                 //If the spanner property is violated for these paths
00686                 if (s_max > stretchFactor_* d_min)
00687                 {
00688                     //Need to augment this path with the appropriate neighbor information
00689                     DenseVertex na = getInterfaceNeighbor(q, vp);
00690                     DenseVertex nb = getInterfaceNeighbor(best_qpp, vpp);
00691 
00692                     bestDPath.push_front( stateProperty_[na] );
00693                     bestDPath.push_back( stateProperty_[nb] );
00694 
00695                     // check consistency of representatives
00696                     assert(representativesProperty_[na] == vp && representativesProperty_[nb] == vpp);
00697 
00698                     //Add the dense path to the spanner
00699                     addPathToSpanner( bestDPath, vpp, vp );
00700 
00701                     //Report success
00702                     result = true;
00703                 }
00704             }
00705         }
00706     }
00707     return result;
00708 }
00709 
00710 double ompl::geometric::SPARS::averageValence() const
00711 {
00712     double degree = 0.0;
00713     foreach (DenseVertex v, boost::vertices(s_))
00714         degree += (double)boost::out_degree(v, s_);
00715     degree /= (double)boost::num_vertices(s_);
00716     return degree;
00717 }
00718 
00719 void ompl::geometric::SPARS::printDebug(std::ostream &out) const
00720 {
00721     out << "SPARS Debug Output: " << std::endl;
00722     out << "  Settings: " << std::endl;
00723     out << "    Max Failures: " << getMaxFailures() << std::endl;
00724     out << "    Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
00725     out << "    Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
00726     out << "    Stretch Factor: " << getStretchFactor() << std::endl;
00727     out << "  Status: " << std::endl;
00728     out << "    Milestone Count: " << milestoneCount() << std::endl;
00729     out << "    Guard Count: " << guardCount() << std::endl;
00730     out << "    Iterations: " << getIterationCount() << std::endl;
00731     out << "    Average Valence: " << averageValence() << std::endl;
00732     out << "    Consecutive Failures: " << consecutiveFailures_ << std::endl;
00733 }
00734 
00735 void ompl::geometric::SPARS::getSparseNeighbors(base::State *inState, std::vector<SparseVertex> &graphNeighborhood)
00736 {
00737     sparseStateProperty_[sparseQueryVertex_] = inState;
00738 
00739     graphNeighborhood.clear();
00740     snn_->nearestR(sparseQueryVertex_, sparseDelta_, graphNeighborhood);
00741 
00742     sparseStateProperty_[sparseQueryVertex_] = NULL;
00743 }
00744 
00745 void ompl::geometric::SPARS::filterVisibleNeighbors(base::State *inState, const std::vector<SparseVertex> &graphNeighborhood,
00746                                                     std::vector<SparseVertex> &visibleNeighborhood) const
00747 {
00748     visibleNeighborhood.clear();
00749     //Now that we got the neighbors from the NN, we must remove any we can't see
00750     for (std::size_t i = 0; i < graphNeighborhood.size(); ++i)
00751         if (si_->checkMotion(inState, sparseStateProperty_[graphNeighborhood[i]]))
00752             visibleNeighborhood.push_back(graphNeighborhood[i]);
00753 }
00754 
00755 ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::getInterfaceNeighbor(DenseVertex q, SparseVertex rep)
00756 {
00757     foreach (DenseVertex vp, boost::adjacent_vertices( q, g_ ))
00758         if (representativesProperty_[vp] == rep )
00759             if (distanceFunction( q, vp ) <= denseDelta_)
00760                 return vp;
00761     throw Exception(name_, "Vertex has no interface neighbor with given representative");
00762 }
00763 
00764 bool ompl::geometric::SPARS::addPathToSpanner( const DensePath &dense_path, SparseVertex vp, SparseVertex vpp )
00765 {
00766     // First, check to see that the path has length
00767     if (dense_path.size() <= 1)
00768     {
00769         // The path is 0 length, so simply link the representatives
00770         connectSparsePoints( vp, vpp );
00771         resetFailures();
00772     }
00773     else
00774     {
00775         //We will need to construct a PathGeometric to do this.
00776         geomPath_.getStates().resize( dense_path.size() );
00777         std::copy( dense_path.begin(), dense_path.end(), geomPath_.getStates().begin() );
00778 
00779         //Attempt to simplify the path
00780         psimp_->reduceVertices( geomPath_, geomPath_.getStateCount() * 2);
00781 
00782         // we are sure there are at least 2 points left on geomPath_
00783 
00784         std::vector< SparseVertex > added_nodes;
00785         added_nodes.reserve(geomPath_.getStateCount());
00786         for (std::size_t i = 0; i < geomPath_.getStateCount(); ++i )
00787         {
00788             //Add each guard
00789             SparseVertex ng = addGuard( si_->cloneState(geomPath_.getState(i)), QUALITY );
00790             added_nodes.push_back( ng );
00791         }
00792         //Link them up
00793         for (std::size_t i = 1; i < added_nodes.size() ; ++i )
00794         {
00795             connectSparsePoints(added_nodes[i - 1], added_nodes[i]);
00796         }
00797         //Don't forget to link them to their representatives
00798         connectSparsePoints( added_nodes[0], vp );
00799         connectSparsePoints( added_nodes[added_nodes.size()-1], vpp );
00800     }
00801     geomPath_.getStates().clear();
00802     return true;
00803 }
00804 
00805 void ompl::geometric::SPARS::updateRepresentatives(SparseVertex v)
00806 {
00807     //Get all of the dense samples which may be affected by adding this node
00808     std::vector< DenseVertex > dense_points;
00809 
00810     stateProperty_[ queryVertex_ ] = sparseStateProperty_[ v ];
00811 
00812     nn_->nearestR( queryVertex_, sparseDelta_ + denseDelta_, dense_points );
00813 
00814     stateProperty_[ queryVertex_ ] = NULL;
00815 
00816     //For each of those points
00817     for (std::size_t i = 0 ; i < dense_points.size() ; ++i)
00818     {
00819         //Remove that point from the old representative's list(s)
00820         removeFromRepresentatives( dense_points[i], representativesProperty_[dense_points[i]] );
00821         //Update that point's representative
00822         calculateRepresentative( dense_points[i] );
00823     }
00824 
00825     std::set<SparseVertex> interfaceRepresentatives;
00826     //For each of the points
00827     for (std::size_t i = 0 ; i < dense_points.size(); ++i)
00828     {
00829         //Get it's representative
00830         SparseVertex rep = representativesProperty_[dense_points[i]];
00831         //Extract the representatives of any interface-sharing neighbors
00832         getInterfaceNeighborRepresentatives( dense_points[i], interfaceRepresentatives );
00833         //For sanity's sake, make sure we clear ourselves out of what this new rep might think of us
00834         removeFromRepresentatives( dense_points[i], rep );
00835         //Add this vertex to it's representative's list for the other representatives
00836         addToRepresentatives( dense_points[i], rep, interfaceRepresentatives );
00837     }
00838 }
00839 
00840 void ompl::geometric::SPARS::calculateRepresentative(DenseVertex q)
00841 {
00842     //Get the nearest neighbors within sparseDelta_
00843     std::vector<SparseVertex> graphNeighborhood;
00844     getSparseNeighbors(stateProperty_[q], graphNeighborhood);
00845 
00846     //For each neighbor
00847     for (std::size_t i = 0; i < graphNeighborhood.size(); ++i)
00848         if (si_->checkMotion(stateProperty_[q], sparseStateProperty_[graphNeighborhood[i]]))
00849         {
00850             //update the representative
00851             representativesProperty_[q] = graphNeighborhood[i];
00852             //abort
00853             break;
00854         }
00855 }
00856 
00857 void ompl::geometric::SPARS::addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set<SparseVertex> &oreps)
00858 {
00859     //If this node supports no interfaces
00860     if (oreps.size() == 0)
00861     {
00862         //Add it to the pool of non-interface nodes
00863         bool new_insert = nonInterfaceListsProperty_[rep].insert(q).second;
00864 
00865         // we expect this was not previously tracked
00866         if (!new_insert)
00867             assert(false);
00868     }
00869     else
00870     {
00871         //otherwise, for every neighbor representative
00872         foreach( SparseVertex v, oreps )
00873         {
00874             assert(rep == representativesProperty_[q]);
00875             bool new_insert = interfaceListsProperty_[rep].interfaceHash[v].insert(q).second;
00876             if (!new_insert)
00877                 assert(false);
00878         }
00879     }
00880 }
00881 
00882 void ompl::geometric::SPARS::removeFromRepresentatives(DenseVertex q, SparseVertex rep)
00883 {
00884     // Remove the node from the non-interface points (if there)
00885     nonInterfaceListsProperty_[rep].erase(q);
00886 
00887     // From each of the interfaces
00888     foreach (SparseVertex vpp, interfaceListsProperty_[rep].interfaceHash | boost::adaptors::map_keys)
00889     {
00890         // Remove this node from that list
00891         interfaceListsProperty_[rep].interfaceHash[vpp].erase( q );
00892     }
00893 }
00894 
00895 void ompl::geometric::SPARS::computeVPP(SparseVertex v, SparseVertex vp, std::vector<SparseVertex> &VPPs)
00896 {
00897     foreach( SparseVertex cvpp, boost::adjacent_vertices( v, s_ ) )
00898         if( cvpp != vp )
00899             if( !boost::edge( cvpp, vp, s_ ).second )
00900                 VPPs.push_back( cvpp );
00901 }
00902 
00903 void ompl::geometric::SPARS::computeX(SparseVertex v, SparseVertex vp, SparseVertex vpp, std::vector<SparseVertex> &Xs)
00904 {
00905     Xs.clear();
00906     foreach( SparseVertex cx, boost::adjacent_vertices( vpp, s_ ) )
00907         if( boost::edge( cx, v, s_ ).second && !boost::edge( cx, vp, s_ ).second )
00908             if (interfaceListsProperty_[vpp].interfaceHash[cx].size() > 0)
00909                 Xs.push_back( cx );
00910     Xs.push_back( vpp );
00911 }
00912 
00913 void ompl::geometric::SPARS::getInterfaceNeighborRepresentatives(DenseVertex q, std::set<SparseVertex> &interfaceRepresentatives)
00914 {
00915     interfaceRepresentatives.clear();
00916 
00917     // Get our representative
00918     SparseVertex rep = representativesProperty_[q];
00919     // For each neighbor we are connected to
00920     foreach( DenseVertex n, boost::adjacent_vertices( q, g_ ) )
00921     {
00922         // Get his representative
00923         SparseVertex orep = representativesProperty_[n];
00924         // If that representative is not our own
00925         if (orep != rep)
00926             // If he is within denseDelta_
00927             if (distanceFunction( q, n ) < denseDelta_)
00928                 // Include his rep in the set
00929                 interfaceRepresentatives.insert(orep);
00930     }
00931 }
00932 
00933 void ompl::geometric::SPARS::getInterfaceNeighborhood(DenseVertex q, std::vector<DenseVertex> &interfaceNeighborhood)
00934 {
00935     interfaceNeighborhood.clear();
00936 
00937     // Get our representative
00938     SparseVertex rep = representativesProperty_[q];
00939 
00940     // For each neighbor we are connected to
00941     foreach( DenseVertex n, boost::adjacent_vertices( q, g_ ) )
00942         // If neighbor representative is not our own
00943         if( representativesProperty_[n] != rep )
00944             // If he is within denseDelta_
00945             if( distanceFunction( q, n ) < denseDelta_ )
00946                 // Append him to the list
00947                 interfaceNeighborhood.push_back( n );
00948 }
00949 
00950 ompl::base::PathPtr ompl::geometric::SPARS::constructSolution(const SparseVertex start, const SparseVertex goal) const
00951 {
00952     boost::mutex::scoped_lock _(graphMutex_);
00953 
00954     boost::vector_property_map<SparseVertex> prev(boost::num_vertices(s_));
00955 
00956     try
00957     {
00958         // Consider using a persistent distance_map if it's slow
00959         boost::astar_search(s_, start,
00960                             boost::bind(&SPARS::costHeuristic, this, _1, goal),
00961                             boost::predecessor_map(prev).
00962                             distance_compare(boost::bind(&base::OptimizationObjective::
00963                                                          isCostBetterThan, opt_.get(), _1, _2)).
00964                             distance_combine(boost::bind(&base::OptimizationObjective::
00965                                                          combineCosts, opt_.get(), _1, _2)).
00966                             distance_inf(opt_->infiniteCost()).
00967                             distance_zero(opt_->identityCost()).
00968                             visitor(AStarGoalVisitor<SparseVertex>(goal)));
00969     }
00970     catch (AStarFoundGoal&)
00971     {
00972     }
00973 
00974     if (prev[goal] == goal)
00975         throw Exception(name_, "Could not find solution path");
00976     else
00977     {
00978         PathGeometric *p = new PathGeometric(si_);
00979 
00980         for (SparseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
00981             p->append(sparseStateProperty_[pos]);
00982         p->append(sparseStateProperty_[start]);
00983         p->reverse();
00984 
00985         return base::PathPtr(p);
00986     }
00987 }
00988 
00989 void ompl::geometric::SPARS::computeDensePath(const DenseVertex start, const DenseVertex goal, DensePath &path) const
00990 {
00991     path.clear();
00992 
00993     boost::vector_property_map<DenseVertex> prev(boost::num_vertices(g_));
00994 
00995     try
00996     {
00997         boost::astar_search(g_, start,
00998                             boost::bind(&SPARS::distanceFunction, this, _1, goal),
00999                             boost::predecessor_map(prev).
01000                             visitor(AStarGoalVisitor<DenseVertex>(goal)));
01001     }
01002     catch (AStarFoundGoal&)
01003     {
01004     }
01005 
01006     if (prev[goal] == goal)
01007         OMPL_WARN("%s: No dense path was found?", getName().c_str());
01008     else
01009     {
01010         for (DenseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
01011             path.push_front( stateProperty_[pos] );
01012         path.push_front( stateProperty_[start] );
01013     }
01014 }
01015 
01016 void ompl::geometric::SPARS::getPlannerData(base::PlannerData &data) const
01017 {
01018     Planner::getPlannerData(data);
01019 
01020     // Explicitly add start and goal states:
01021     for (std::size_t i = 0; i < startM_.size(); ++i)
01022         data.addStartVertex(base::PlannerDataVertex(sparseStateProperty_[startM_[i]], (int)START));
01023 
01024     for (std::size_t i = 0; i < goalM_.size(); ++i)
01025         data.addGoalVertex(base::PlannerDataVertex(sparseStateProperty_[goalM_[i]], (int)GOAL));
01026 
01027     // Adding edges and all other vertices simultaneously
01028     foreach (const SparseEdge e, boost::edges(s_))
01029     {
01030         const SparseVertex v1 = boost::source(e, s_);
01031         const SparseVertex v2 = boost::target(e, s_);
01032         data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v1]),
01033                      base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v2]));
01034 
01035         // Add the reverse edge, since we're constructing an undirected roadmap
01036         data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v1]),
01037                      base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v2]));
01038     }
01039 
01040     // Make sure to add edge-less nodes as well
01041     foreach (const SparseVertex n, boost::vertices(s_))
01042         if (boost::out_degree( n, s_ ) == 0)
01043             data.addVertex(base::PlannerDataVertex(sparseStateProperty_[n], (int)sparseColorProperty_[n]));
01044 }
01045 
01046 ompl::base::Cost ompl::geometric::SPARS::costHeuristic(SparseVertex u, SparseVertex v) const
01047 {
01048     return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
01049 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines