All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
Syclop.h
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: Matt Maly */
00036 
00037 #ifndef OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
00038 #define OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
00039 
00040 #include <boost/graph/astar_search.hpp>
00041 #include <boost/graph/graph_traits.hpp>
00042 #include <boost/graph/adjacency_list.hpp>
00043 #include <boost/unordered_map.hpp>
00044 #include "ompl/control/planners/PlannerIncludes.h"
00045 #include "ompl/control/planners/syclop/Decomposition.h"
00046 #include "ompl/control/planners/syclop/GridDecomposition.h"
00047 #include "ompl/datastructures/PDF.h"
00048 #include <map>
00049 #include <vector>
00050 
00051 namespace ompl
00052 {
00053     namespace control
00054     {
00072         class Syclop : public base::Planner
00073         {
00074         public:
00090             typedef boost::function2<double, int, int> EdgeCostFactorFn;
00091 
00093             Syclop(const SpaceInformationPtr& si, const DecompositionPtr &d, const std::string& plannerName) : ompl::base::Planner(si, plannerName),
00094                 numFreeVolSamples_(Defaults::NUM_FREEVOL_SAMPLES),
00095                 probShortestPath_(Defaults::PROB_SHORTEST_PATH),
00096                 probKeepAddingToAvail_(Defaults::PROB_KEEP_ADDING_TO_AVAIL),
00097                 numRegionExpansions_(Defaults::NUM_REGION_EXPANSIONS),
00098                 numTreeSelections_(Defaults::NUM_TREE_SELECTIONS),
00099                 probAbandonLeadEarly_(Defaults::PROB_ABANDON_LEAD_EARLY),
00100                 siC_(si.get()),
00101                 decomp_(d),
00102                 covGrid_(Defaults::COVGRID_LENGTH, decomp_),
00103                 graphReady_(false),
00104                 numMotions_(0)
00105             {
00106                 specs_.approximateSolutions = true;
00107 
00108                 Planner::declareParam<int>   ("free_volume_samples", this, &Syclop::setNumFreeVolumeSamples, &Syclop::getNumFreeVolumeSamples);
00109                 Planner::declareParam<int>   ("num_region_expansions", this, &Syclop::setNumRegionExpansions, &Syclop::getNumRegionExpansions);
00110                 Planner::declareParam<int>   ("num_tree_expansions", this, &Syclop::setNumTreeExpansions, &Syclop::getNumTreeExpansions);
00111                 Planner::declareParam<double>("prob_abandon_lead_early", this, &Syclop::setProbAbandonLeadEarly, &Syclop::getProbAbandonLeadEarly);
00112                 Planner::declareParam<double>("prob_add_available_regions", this, &Syclop::setProbAddingToAvailableRegions, &Syclop::getProbAddingToAvailableRegions);
00113                 Planner::declareParam<double>("prob_shortest_path_lead", this, &Syclop::setProbShortestPathLead, &Syclop::getProbShortestPathLead);
00114             }
00115 
00116             virtual ~Syclop()
00117             {
00118             }
00119 
00122 
00123             virtual void setup(void);
00124 
00125             virtual void clear(void);
00126 
00129             virtual bool solve(const base::PlannerTerminationCondition& ptc);
00131 
00134 
00136             void addEdgeCostFactor(const EdgeCostFactorFn& factor);
00137 
00139             void clearEdgeCostFactors(void);
00140 
00142             int getNumFreeVolumeSamples (void) const
00143             {
00144                 return numFreeVolSamples_;
00145             }
00146 
00149             void setNumFreeVolumeSamples (int numSamples)
00150             {
00151                 numFreeVolSamples_ = numSamples;
00152             }
00153 
00156             double getProbShortestPathLead (void) const
00157             {
00158                 return probShortestPath_;
00159             }
00160 
00163             void setProbShortestPathLead (double probability)
00164             {
00165                 probShortestPath_ = probability;
00166             }
00167 
00170             double getProbAddingToAvailableRegions (void) const
00171             {
00172                 return probKeepAddingToAvail_;
00173             }
00174 
00177             void setProbAddingToAvailableRegions (double probability)
00178             {
00179                 probKeepAddingToAvail_ = probability;
00180             }
00181 
00184             int getNumRegionExpansions (void) const
00185             {
00186                 return numRegionExpansions_;
00187             }
00188 
00191             void setNumRegionExpansions (int regionExpansions)
00192             {
00193                 numRegionExpansions_ = regionExpansions;
00194             }
00195 
00198             int getNumTreeExpansions (void) const
00199             {
00200                 return numTreeSelections_;
00201             }
00202 
00205             void setNumTreeExpansions (int treeExpansions)
00206             {
00207                 numTreeSelections_ = treeExpansions;
00208             }
00209 
00212             double getProbAbandonLeadEarly (void) const
00213             {
00214                 return probAbandonLeadEarly_;
00215             }
00216 
00219             void setProbAbandonLeadEarly (double probability)
00220             {
00221                 probAbandonLeadEarly_ = probability;
00222             }
00224 
00226             struct Defaults
00227             {
00228                 static const int    NUM_FREEVOL_SAMPLES         = 100000;
00229                 static const int    COVGRID_LENGTH              = 128;
00230                 static const int    NUM_REGION_EXPANSIONS       = 100;
00231                 static const int    NUM_TREE_SELECTIONS         = 50;
00232                 // C++ standard prohibits non-integral static const member initialization
00233                 // These constants are set in Syclop.cpp.  C++11 standard changes this
00234                 // with the constexpr keyword, but for compatibility this is not done.
00235                 static const double PROB_ABANDON_LEAD_EARLY     /*= 0.25*/;
00236                 static const double PROB_KEEP_ADDING_TO_AVAIL   /*= 0.95*/;
00237                 static const double PROB_SHORTEST_PATH          /*= 0.95*/;
00238             };
00239 
00240         protected:
00241 
00242             #pragma pack(push, 4)  // push default byte alignment to stack and align the following structure to 4 byte boundary
00243 
00247             class Motion
00248             {
00249             public:
00250                 Motion(void) : state(NULL), control(NULL), parent(NULL), steps(0)
00251                 {
00252                 }
00254                 Motion(const SpaceInformation* si) : state(si->allocState()), control(si->allocControl()), parent(NULL), steps(0)
00255                 {
00256                 }
00257                 virtual ~Motion(void)
00258                 {
00259                 }
00261                 base::State* state;
00263                 Control* control;
00265                 const Motion* parent;
00267                 unsigned int steps;
00268             };
00269             #pragma pack (pop)  // Restoring default byte alignment
00270 
00271             #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
00272 
00273             class Region
00274             {
00275             public:
00276                 Region(void)
00277                 {
00278                 }
00279                 virtual ~Region(void)
00280                 {
00281                 }
00283                 void clear(void)
00284                 {
00285                     motions.clear();
00286                     covGridCells.clear();
00287                 }
00288 
00290                 std::set<int> covGridCells;
00292                 std::vector<Motion*> motions;
00294                 double volume;
00296                 double freeVolume;
00298                 double percentValidCells;
00300                 double weight;
00302                 double alpha;
00304                 int index;
00306                 unsigned int numSelections;
00307             };
00308             #pragma pack (pop)  // Restoring default byte alignment
00309 
00310             #pragma pack(push, 4)  // push default byte alignment to stack and align the following structure to 4 byte boundary
00311 
00313             class Adjacency
00314             {
00315             public:
00316                 Adjacency(void)
00317                 {
00318                 }
00319                 virtual ~Adjacency(void)
00320                 {
00321                 }
00323                 void clear(void)
00324                 {
00325                     covGridCells.clear();
00326                 }
00329                 std::set<int> covGridCells;
00331                 const Region* source;
00333                 const Region* target;
00335                 double cost;
00337                 int numLeadInclusions;
00340                 int numSelections;
00342                 bool empty;
00343             };
00344             #pragma pack (pop) // Restoring default byte alignment
00345 
00347             virtual Motion* addRoot(const base::State* s) = 0;
00348 
00351             virtual void selectAndExtend(Region& region, std::vector<Motion*>& newMotions) = 0;
00352 
00354             inline const Region& getRegionFromIndex(const int rid) const
00355             {
00356                 return graph_[boost::vertex(rid,graph_)];
00357             }
00358 
00360             int numFreeVolSamples_;
00361 
00363             double probShortestPath_;
00364 
00366             double probKeepAddingToAvail_;
00367 
00369             int numRegionExpansions_;
00370 
00372             int numTreeSelections_;
00373 
00375             double probAbandonLeadEarly_;
00376 
00378             const SpaceInformation* siC_;
00379 
00381             DecompositionPtr decomp_;
00382 
00384             RNG rng_;
00385 
00386         private:
00389             class CoverageGrid : public GridDecomposition
00390             {
00391             public:
00392                 CoverageGrid(const int len, const DecompositionPtr& d) : GridDecomposition(len, d->getDimension(), d->getBounds()), decomp(d)
00393                 {
00394                 }
00395 
00396                 virtual ~CoverageGrid()
00397                 {
00398                 }
00399 
00402                 virtual void project(const base::State* s, std::vector<double>& coord) const
00403                 {
00404                     decomp->project(s, coord);
00405                 }
00406 
00408                 virtual void sampleFromRegion(const int rid, const base::StateSamplerPtr& sampler, base::State* s)
00409                 {
00410                 }
00411 
00412             protected:
00413                 const DecompositionPtr& decomp;
00414             };
00415 
00416             typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
00417             typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
00418             typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
00419             typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
00420             typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;
00421 
00423             friend class DecompositionHeuristic;
00424 
00425             class DecompositionHeuristic : public boost::astar_heuristic<RegionGraph, double>
00426             {
00427             public:
00428                 DecompositionHeuristic(const Syclop* s, const Region& goal) : syclop(s), goalRegion(goal)
00429                 {
00430                 }
00431 
00432                 double operator()(Vertex v)
00433                 {
00434                     const Region& region = syclop->getRegionFromIndex(v);
00435                     return region.alpha*goalRegion.alpha;
00436                 }
00437             private:
00438                 const Syclop* syclop;
00439                 const Region& goalRegion;
00440             };
00441 
00442             struct found_goal {};
00443 
00444             class GoalVisitor : public boost::default_astar_visitor
00445             {
00446             public:
00447                 GoalVisitor(const unsigned int goal) : goalRegion(goal)
00448                 {
00449                 }
00450                 void examine_vertex(Vertex v, const RegionGraph& g)
00451                 {
00452                     if (v == goalRegion)
00453                         throw found_goal();
00454                 }
00455             private:
00456                 const unsigned int goalRegion;
00457             };
00459 
00461             class RegionSet
00462             {
00463             public:
00464                 int sampleUniform(void)
00465                 {
00466                     if (empty())
00467                         return -1;
00468                     return regions.sample(rng.uniform01());
00469                 }
00470                 void insert(const int r)
00471                 {
00472                     if (regToElem.count(r) == 0)
00473                         regToElem[r] = regions.add(r, 1);
00474                     else
00475                     {
00476                         PDF<int>::Element* elem = regToElem[r];
00477                         regions.update(elem, regions.getWeight(elem)+1);
00478                     }
00479                 }
00480                 void clear(void)
00481                 {
00482                     regions.clear();
00483                     regToElem.clear();
00484                 }
00485                 std::size_t size(void) const
00486                 {
00487                     return regions.size();
00488                 }
00489                 bool empty() const
00490                 {
00491                     return regions.empty();
00492                 }
00493             private:
00494                 RNG rng;
00495                 PDF<int> regions;
00496                 boost::unordered_map<const int, PDF<int>::Element*> regToElem;
00497             };
00499 
00501             void initRegion(Region& r);
00502 
00504             void setupRegionEstimates(void);
00505 
00507             void updateRegion(Region& r);
00508 
00510             void initEdge(Adjacency& a, const Region* source, const Region* target);
00511 
00513             void setupEdgeEstimates(void);
00514 
00516             void updateEdge(Adjacency& a);
00517 
00520             bool updateCoverageEstimate(Region& r, const base::State* s);
00521 
00524             bool updateConnectionEstimate(const Region& c, const Region& d, const base::State* s);
00525 
00528             void buildGraph(void);
00529 
00531             void clearGraphDetails(void);
00532 
00534             void computeLead(int startRegion, int goalRegion);
00535 
00537             int selectRegion(void);
00538 
00540             void computeAvailableRegions(void);
00541 
00543             double defaultEdgeCost(int r, int s);
00544 
00546             std::vector<int> lead_;
00548             PDF<int> availDist_;
00550             std::vector<EdgeCostFactorFn> edgeCostFactors_;
00552             CoverageGrid covGrid_;
00554             RegionGraph graph_;
00556             bool graphReady_;
00558             boost::unordered_map<std::pair<int,int>, Adjacency*> regionsToEdge_;
00560             unsigned int numMotions_;
00562             RegionSet startRegions_;
00564             RegionSet goalRegions_;
00565         };
00566     }
00567 }
00568 
00569 #endif