ompl/geometric/planners/prm/SPARS.h
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 #ifndef OMPL_GEOMETRIC_PLANNERS_SPARSE_ROADMAP_SPANNER_
00038 #define OMPL_GEOMETRIC_PLANNERS_SPARSE_ROADMAP_SPANNER_
00039 
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/datastructures/NearestNeighbors.h"
00042 #include "ompl/geometric/PathSimplifier.h"
00043 #include "ompl/util/Time.h"
00044 
00045 #include <boost/range/adaptor/map.hpp>
00046 #include <boost/unordered_map.hpp>
00047 #include <boost/graph/graph_traits.hpp>
00048 #include <boost/graph/adjacency_list.hpp>
00049 #include <boost/pending/disjoint_sets.hpp>
00050 #include <boost/function.hpp>
00051 #include <boost/thread.hpp>
00052 #include <iostream>
00053 #include <fstream>
00054 #include <utility>
00055 #include <vector>
00056 #include <deque>
00057 #include <map>
00058 #include <set>
00059 
00060 namespace ompl
00061 {
00062 
00063     namespace geometric
00064     {
00065 
00080         class SPARS : public base::Planner
00081         {
00082         public:
00084             enum GuardType
00085             {
00086                 START,
00087                 GOAL,
00088                 COVERAGE,
00089                 CONNECTIVITY,
00090                 INTERFACE,
00091                 QUALITY,
00092             };
00093 
00094             struct vertex_state_t {
00095                 typedef boost::vertex_property_tag kind;
00096             };
00097 
00098             struct vertex_representative_t {
00099                 typedef boost::vertex_property_tag kind;
00100             };
00101 
00102             struct vertex_color_t {
00103                 typedef boost::vertex_property_tag kind;
00104             };
00105 
00106             struct vertex_list_t {
00107                 typedef boost::vertex_property_tag kind;
00108             };
00109 
00110             struct vertex_interface_list_t {
00111                 typedef boost::vertex_property_tag kind;
00112             };
00113 
00115             typedef unsigned long int VertexIndexType;
00116 
00118             typedef boost::unordered_map<VertexIndexType, std::set<VertexIndexType>, boost::hash<VertexIndexType> > InterfaceHash;
00119 
00121             typedef std::deque<base::State*> DensePath;
00122 
00123             // The InterfaceHash structure is wrapped inside of this struct due to a compilation error on
00124             // GCC 4.6 with Boost 1.48.  An implicit assignment operator overload does not compile with these
00125             // components, so an explicit overload is given here.
00126             // Remove this struct when the minimum Boost requirement is > v1.48.
00127             struct InterfaceHashStruct
00128             {
00129                 InterfaceHashStruct& operator=(const InterfaceHashStruct &rhs) { interfaceHash = rhs.interfaceHash; return *this; }
00130                 InterfaceHash interfaceHash;
00131             };
00132 
00145             typedef boost::adjacency_list <
00146                 boost::vecS, boost::vecS, boost::undirectedS,
00147                 boost::property < vertex_state_t, base::State*,
00148                 boost::property < boost::vertex_predecessor_t, VertexIndexType,
00149                 boost::property < boost::vertex_rank_t, VertexIndexType,
00150                 boost::property < vertex_color_t, GuardType,
00151                 boost::property < vertex_list_t, std::set<VertexIndexType>,
00152                 boost::property < vertex_interface_list_t, InterfaceHashStruct > > > > > >,
00153                 boost::property < boost::edge_weight_t, base::Cost >
00154             > SpannerGraph;
00155 
00157             typedef boost::graph_traits<SpannerGraph>::vertex_descriptor SparseVertex;
00158 
00160             typedef boost::graph_traits<SpannerGraph>::edge_descriptor   SparseEdge;
00161 
00163             typedef boost::shared_ptr< NearestNeighbors<SparseVertex> > SparseNeighbors;
00164 
00180             typedef boost::adjacency_list <
00181                 boost::vecS, boost::vecS, boost::undirectedS,
00182                 boost::property < vertex_state_t, base::State*,
00183                 boost::property < boost::vertex_predecessor_t, VertexIndexType,
00184                 boost::property < boost::vertex_rank_t, VertexIndexType,
00185                 boost::property < vertex_representative_t, SparseVertex > > > >,
00186                 boost::property < boost::edge_weight_t, double >
00187             > DenseGraph;
00188 
00190             typedef boost::graph_traits<DenseGraph>::vertex_descriptor DenseVertex;
00191 
00193             typedef boost::graph_traits<DenseGraph>::edge_descriptor   DenseEdge;
00194 
00196             typedef boost::shared_ptr< NearestNeighbors<DenseVertex> > DenseNeighbors;
00197 
00199             SPARS(const base::SpaceInformationPtr &si);
00201             virtual ~SPARS();
00202 
00203             virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef);
00204 
00205             virtual void getPlannerData(base::PlannerData &data) const;
00206 
00208             void constructRoadmap(const base::PlannerTerminationCondition &ptc);
00209 
00212             void constructRoadmap(const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail);
00213 
00226             virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00227 
00232             void clearQuery();
00233 
00234             virtual void clear();
00235 
00241             template<template<typename T> class NN>
00242             void setDenseNeighbors()
00243             {
00244                 nn_.reset(new NN<DenseVertex>());
00245                 connectionStrategy_.clear();
00246                 if (isSetup())
00247                     setup();
00248             }
00249 
00254             template<template<typename T> class NN>
00255             void setSparseNeighbors()
00256             {
00257                 snn_.reset(new NN<SparseVertex>());
00258                 if (isSetup())
00259                     setup();
00260             }
00261 
00266             void setMaxFailures(unsigned int m)
00267             {
00268                 maxFailures_ = m;
00269             }
00270 
00274             void setDenseDeltaFraction(double d)
00275             {
00276                 denseDeltaFraction_ = d;
00277                 if (denseDelta_ > 0.0) // setup was previously called
00278                     denseDelta_ = d * si_->getMaximumExtent();
00279             }
00280 
00285             void setSparseDeltaFraction(double d)
00286             {
00287                 sparseDeltaFraction_ = d;
00288                 if (sparseDelta_ > 0.0) // setup was previously called
00289                     sparseDelta_ = d * si_->getMaximumExtent();
00290             }
00291 
00295             void setStretchFactor(double t)
00296             {
00297                 stretchFactor_ = t;
00298             }
00299 
00301             unsigned getMaxFailures() const
00302             {
00303                 return maxFailures_;
00304             }
00305 
00307             double getDenseDeltaFraction() const
00308             {
00309                 return denseDeltaFraction_;
00310             }
00311 
00313             double getSparseDeltaFraction() const
00314             {
00315                 return sparseDeltaFraction_;
00316             }
00317 
00319             double getStretchFactor() const
00320             {
00321                 return stretchFactor_;
00322             }
00323 
00324             virtual void setup();
00325 
00327             const DenseGraph& getDenseGraph() const
00328             {
00329                 return g_;
00330             }
00331 
00334             const SpannerGraph& getRoadmap() const
00335             {
00336                 return s_;
00337             }
00338 
00340             unsigned int milestoneCount() const
00341             {
00342                 return boost::num_vertices(g_);
00343             }
00344 
00346             unsigned int guardCount() const
00347             {
00348                 return boost::num_vertices(s_);
00349             }
00350 
00352             double averageValence() const;
00353 
00355             void printDebug(std::ostream &out = std::cout) const;
00356 
00358             bool reachedFailureLimit() const;
00359 
00361             // Planner progress property functions
00362             std::string getIterationCount() const
00363             {
00364                 return boost::lexical_cast<std::string>(iterations_);
00365             }
00366             std::string getBestCost() const
00367             {
00368                 return boost::lexical_cast<std::string>(bestCost_);
00369             }
00370 
00371         protected:
00372 
00374             DenseVertex addSample(base::State *workState, const base::PlannerTerminationCondition &ptc);
00375 
00377             void checkQueryStateInitialization();
00378 
00380             bool sameComponent(SparseVertex m1, SparseVertex m2);
00381 
00383             DenseVertex addMilestone(base::State *state);
00384 
00386             SparseVertex addGuard(base::State *state, GuardType type);
00387 
00389             void connectSparsePoints(SparseVertex v, SparseVertex vp);
00390 
00392             void connectDensePoints(DenseVertex v, DenseVertex vp);
00393 
00395             bool checkAddCoverage(const base::State *lastState, const std::vector<SparseVertex> &neigh);
00396 
00398             bool checkAddConnectivity(const base::State *lastState, const std::vector<SparseVertex> &neigh);
00399 
00401             bool checkAddInterface(const std::vector<DenseVertex>& graphNeighborhood, const std::vector<DenseVertex>& visibleNeighborhood, DenseVertex q);
00402 
00404             bool checkAddPath( DenseVertex q, const std::vector<DenseVertex>& neigh );
00405 
00407             DenseVertex getInterfaceNeighbor(DenseVertex q, SparseVertex rep);
00408 
00410             bool addPathToSpanner(const DensePath &p, SparseVertex vp, SparseVertex vpp);
00411 
00413             void updateRepresentatives(SparseVertex v);
00414 
00416             void calculateRepresentative( DenseVertex q );
00417 
00419             void addToRepresentatives( DenseVertex q, SparseVertex rep, const std::set<SparseVertex>& oreps );
00420 
00422             void removeFromRepresentatives( DenseVertex q, SparseVertex rep );
00423 
00425             void computeVPP(DenseVertex v, DenseVertex vp, std::vector<SparseVertex> &VPPs);
00426 
00428             void computeX(DenseVertex v, DenseVertex vp, DenseVertex vpp, std::vector<SparseVertex> &Xs);
00429 
00431             void resetFailures();
00432 
00434             void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution);
00435 
00437             bool haveSolution(const std::vector<DenseVertex> &start, const std::vector<DenseVertex> &goal, base::PathPtr &solution);
00438 
00440             bool reachedTerminationCriterion() const;
00441 
00443             base::PathPtr constructSolution(const SparseVertex start, const SparseVertex goal) const;
00444 
00446             void computeDensePath(const DenseVertex start, const DenseVertex goal, DensePath &path) const;
00447 
00449             void freeMemory();
00450 
00452             void getSparseNeighbors(base::State *inState, std::vector< SparseVertex > &graphNeighborhood);
00453 
00455             void filterVisibleNeighbors(base::State *inState, const std::vector<SparseVertex> &graphNeighborhood, std::vector<SparseVertex> &visibleNeighborhood) const;
00456 
00458             void getInterfaceNeighborRepresentatives(DenseVertex q, std::set<SparseVertex> &interfaceRepresentatives);
00459 
00461             void getInterfaceNeighborhood(DenseVertex q, std::vector<DenseVertex> &interfaceNeighborhood);
00462 
00464             double distanceFunction(const DenseVertex a, const DenseVertex b) const
00465             {
00466                 return si_->distance(stateProperty_[a], stateProperty_[b]);
00467             }
00468 
00470             double sparseDistanceFunction( const SparseVertex a, const SparseVertex b ) const
00471             {
00472                 return si_->distance( sparseStateProperty_[a], sparseStateProperty_[b] );
00473             }
00474 
00476             base::ValidStateSamplerPtr                                          sampler_;
00477 
00479             base::StateSamplerPtr                                               simpleSampler_;
00480 
00482             DenseNeighbors                                                      nn_;
00483 
00485             SparseNeighbors                                                     snn_;
00486 
00488             DenseGraph                                                          g_;
00489 
00491             SpannerGraph                                                        s_;
00492 
00494             std::vector<SparseVertex>                                           startM_;
00495 
00497             std::vector<SparseVertex>                                           goalM_;
00498 
00500             DenseVertex                                                         sparseQueryVertex_;
00501 
00503             DenseVertex                                                         queryVertex_;
00504 
00506             PathGeometric                                                       geomPath_;
00507 
00509             boost::property_map<DenseGraph, vertex_state_t>::type               stateProperty_;
00510 
00512             boost::property_map<SpannerGraph, vertex_state_t>::type             sparseStateProperty_;
00513 
00515             boost::property_map<SpannerGraph, vertex_color_t>::type             sparseColorProperty_;
00516 
00518             boost::property_map<DenseGraph, vertex_representative_t>::type      representativesProperty_;
00519 
00521             boost::property_map<SpannerGraph, vertex_list_t>::type              nonInterfaceListsProperty_;
00522 
00524             boost::property_map<SpannerGraph, vertex_interface_list_t>::type    interfaceListsProperty_;
00525 
00527             PathSimplifierPtr                                                   psimp_;
00528 
00530             boost::property_map<DenseGraph, boost::edge_weight_t>::type         weightProperty_;
00531 
00533             boost::disjoint_sets<
00534                 boost::property_map<SpannerGraph, boost::vertex_rank_t>::type,
00535                 boost::property_map<SpannerGraph, boost::vertex_predecessor_t>::type >
00536                                                                                 sparseDJSets_;
00537 
00539             boost::function<const std::vector<DenseVertex>&(const DenseVertex)> connectionStrategy_;
00540 
00542             unsigned int                                                        consecutiveFailures_;
00543 
00545             double                                                              stretchFactor_;
00546 
00548             unsigned int                                                        maxFailures_;
00549 
00551             bool                                                                addedSolution_;
00552 
00554             double                                                              denseDeltaFraction_;
00555 
00557             double                                                              sparseDeltaFraction_;
00558 
00560             double                                                              denseDelta_;
00561 
00563             double                                                              sparseDelta_;
00564 
00566             RNG                                                                 rng_;
00567 
00569             mutable boost::mutex                                                graphMutex_;
00570 
00572             base::OptimizationObjectivePtr                                      opt_;
00573 
00575             base::Cost costHeuristic(SparseVertex u, SparseVertex v) const;
00576 
00578             // Planner progress properties
00580             long unsigned int                                                   iterations_;
00582             base::Cost                                                          bestCost_;
00583         };
00584 
00585     }
00586 }
00587 
00588 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines