All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/control/planners/syclop/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     {
00070         class Syclop : public base::Planner
00071         {
00072         public:
00088             typedef boost::function<double(int, int)> EdgeCostFactorFn;
00089 
00091             typedef boost::function<void(int, int, std::vector<int>&)> LeadComputeFn;
00092 
00094             Syclop(const SpaceInformationPtr& si, const DecompositionPtr &d, const std::string& plannerName) : ompl::base::Planner(si, plannerName),
00095                 numFreeVolSamples_(Defaults::NUM_FREEVOL_SAMPLES),
00096                 probShortestPath_(Defaults::PROB_SHORTEST_PATH),
00097                 probKeepAddingToAvail_(Defaults::PROB_KEEP_ADDING_TO_AVAIL),
00098                 numRegionExpansions_(Defaults::NUM_REGION_EXPANSIONS),
00099                 numTreeSelections_(Defaults::NUM_TREE_SELECTIONS),
00100                 probAbandonLeadEarly_(Defaults::PROB_ABANDON_LEAD_EARLY),
00101                 siC_(si.get()),
00102                 decomp_(d),
00103                 covGrid_(Defaults::COVGRID_LENGTH, decomp_),
00104                 graphReady_(false),
00105                 numMotions_(0)
00106             {
00107                 specs_.approximateSolutions = true;
00108 
00109                 Planner::declareParam<int>   ("free_volume_samples", this, &Syclop::setNumFreeVolumeSamples, &Syclop::getNumFreeVolumeSamples);
00110                 Planner::declareParam<int>   ("num_region_expansions", this, &Syclop::setNumRegionExpansions, &Syclop::getNumRegionExpansions);
00111                 Planner::declareParam<int>   ("num_tree_expansions", this, &Syclop::setNumTreeExpansions, &Syclop::getNumTreeExpansions);
00112                 Planner::declareParam<double>("prob_abandon_lead_early", this, &Syclop::setProbAbandonLeadEarly, &Syclop::getProbAbandonLeadEarly);
00113                 Planner::declareParam<double>("prob_add_available_regions", this, &Syclop::setProbAddingToAvailableRegions, &Syclop::getProbAddingToAvailableRegions);
00114                 Planner::declareParam<double>("prob_shortest_path_lead", this, &Syclop::setProbShortestPathLead, &Syclop::getProbShortestPathLead);
00115             }
00116 
00117             virtual ~Syclop()
00118             {
00119             }
00120 
00123 
00124             virtual void setup(void);
00125 
00126             virtual void clear(void);
00127 
00130             virtual base::PlannerStatus solve(const base::PlannerTerminationCondition& ptc);
00132 
00135 
00137             void setLeadComputeFn(const LeadComputeFn& compute);
00138 
00140             void addEdgeCostFactor(const EdgeCostFactorFn& factor);
00141 
00143             void clearEdgeCostFactors(void);
00144 
00146             int getNumFreeVolumeSamples (void) const
00147             {
00148                 return numFreeVolSamples_;
00149             }
00150 
00153             void setNumFreeVolumeSamples (int numSamples)
00154             {
00155                 numFreeVolSamples_ = numSamples;
00156             }
00157 
00160             double getProbShortestPathLead (void) const
00161             {
00162                 return probShortestPath_;
00163             }
00164 
00167             void setProbShortestPathLead (double probability)
00168             {
00169                 probShortestPath_ = probability;
00170             }
00171 
00174             double getProbAddingToAvailableRegions (void) const
00175             {
00176                 return probKeepAddingToAvail_;
00177             }
00178 
00181             void setProbAddingToAvailableRegions (double probability)
00182             {
00183                 probKeepAddingToAvail_ = probability;
00184             }
00185 
00188             int getNumRegionExpansions (void) const
00189             {
00190                 return numRegionExpansions_;
00191             }
00192 
00195             void setNumRegionExpansions (int regionExpansions)
00196             {
00197                 numRegionExpansions_ = regionExpansions;
00198             }
00199 
00202             int getNumTreeExpansions (void) const
00203             {
00204                 return numTreeSelections_;
00205             }
00206 
00209             void setNumTreeExpansions (int treeExpansions)
00210             {
00211                 numTreeSelections_ = treeExpansions;
00212             }
00213 
00216             double getProbAbandonLeadEarly (void) const
00217             {
00218                 return probAbandonLeadEarly_;
00219             }
00220 
00223             void setProbAbandonLeadEarly (double probability)
00224             {
00225                 probAbandonLeadEarly_ = probability;
00226             }
00228 
00230             struct Defaults
00231             {
00232                 static const int    NUM_FREEVOL_SAMPLES         = 100000;
00233                 static const int    COVGRID_LENGTH              = 128;
00234                 static const int    NUM_REGION_EXPANSIONS       = 100;
00235                 static const int    NUM_TREE_SELECTIONS         = 50;
00236                 // C++ standard prohibits non-integral static const member initialization
00237                 // These constants are set in Syclop.cpp.  C++11 standard changes this
00238                 // with the constexpr keyword, but for compatibility this is not done.
00239                 static const double PROB_ABANDON_LEAD_EARLY     /*= 0.25*/;
00240                 static const double PROB_KEEP_ADDING_TO_AVAIL   /*= 0.95*/;
00241                 static const double PROB_SHORTEST_PATH          /*= 0.95*/;
00242             };
00243 
00244         protected:
00245 
00246             #pragma pack(push, 4)  // push default byte alignment to stack and align the following structure to 4 byte boundary
00247 
00251             class Motion
00252             {
00253             public:
00254                 Motion(void) : state(NULL), control(NULL), parent(NULL), steps(0)
00255                 {
00256                 }
00258                 Motion(const SpaceInformation* si) : state(si->allocState()), control(si->allocControl()), parent(NULL), steps(0)
00259                 {
00260                 }
00261                 virtual ~Motion(void)
00262                 {
00263                 }
00265                 base::State* state;
00267                 Control* control;
00269                 const Motion* parent;
00271                 unsigned int steps;
00272             };
00273             #pragma pack (pop)  // Restoring default byte alignment
00274 
00275             #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
00276 
00277             class Region
00278             {
00279             public:
00280                 Region(void)
00281                 {
00282                 }
00283                 virtual ~Region(void)
00284                 {
00285                 }
00287                 void clear(void)
00288                 {
00289                     motions.clear();
00290                     covGridCells.clear();
00291                 }
00292 
00294                 std::set<int> covGridCells;
00296                 std::vector<Motion*> motions;
00298                 double volume;
00300                 double freeVolume;
00302                 double percentValidCells;
00304                 double weight;
00306                 double alpha;
00308                 int index;
00310                 unsigned int numSelections;
00311             };
00312             #pragma pack (pop)  // Restoring default byte alignment
00313 
00314             #pragma pack(push, 4)  // push default byte alignment to stack and align the following structure to 4 byte boundary
00315 
00317             class Adjacency
00318             {
00319             public:
00320                 Adjacency(void)
00321                 {
00322                 }
00323                 virtual ~Adjacency(void)
00324                 {
00325                 }
00327                 void clear(void)
00328                 {
00329                     covGridCells.clear();
00330                 }
00333                 std::set<int> covGridCells;
00335                 const Region* source;
00337                 const Region* target;
00339                 double cost;
00341                 int numLeadInclusions;
00344                 int numSelections;
00346                 bool empty;
00347             };
00348             #pragma pack (pop) // Restoring default byte alignment
00349 
00351             virtual Motion* addRoot(const base::State* s) = 0;
00352 
00355             virtual void selectAndExtend(Region& region, std::vector<Motion*>& newMotions) = 0;
00356 
00358             inline const Region& getRegionFromIndex(const int rid) const
00359             {
00360                 return graph_[boost::vertex(rid,graph_)];
00361             }
00362 
00364             int numFreeVolSamples_;
00365 
00367             double probShortestPath_;
00368 
00370             double probKeepAddingToAvail_;
00371 
00373             int numRegionExpansions_;
00374 
00376             int numTreeSelections_;
00377 
00379             double probAbandonLeadEarly_;
00380 
00382             const SpaceInformation* siC_;
00383 
00385             DecompositionPtr decomp_;
00386 
00388             RNG rng_;
00389 
00390         private:
00393             class CoverageGrid : public GridDecomposition
00394             {
00395             public:
00396                 CoverageGrid(const int len, const DecompositionPtr& d) : GridDecomposition(len, d->getDimension(), d->getBounds()), decomp(d)
00397                 {
00398                 }
00399 
00400                 virtual ~CoverageGrid()
00401                 {
00402                 }
00403 
00406                 virtual void project(const base::State* s, std::vector<double>& coord) const
00407                 {
00408                     decomp->project(s, coord);
00409                 }
00410 
00412                 virtual void sampleFromRegion(const int rid, const base::StateSamplerPtr& sampler, base::State* s)
00413                 {
00414                 }
00415 
00416             protected:
00417                 const DecompositionPtr& decomp;
00418             };
00419 
00420             typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
00421             typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
00422             typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
00423             typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
00424             typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;
00425 
00427             friend class DecompositionHeuristic;
00428 
00429             class DecompositionHeuristic : public boost::astar_heuristic<RegionGraph, double>
00430             {
00431             public:
00432                 DecompositionHeuristic(const Syclop* s, const Region& goal) : syclop(s), goalRegion(goal)
00433                 {
00434                 }
00435 
00436                 double operator()(Vertex v)
00437                 {
00438                     const Region& region = syclop->getRegionFromIndex(v);
00439                     return region.alpha*goalRegion.alpha;
00440                 }
00441             private:
00442                 const Syclop* syclop;
00443                 const Region& goalRegion;
00444             };
00445 
00446             struct found_goal {};
00447 
00448             class GoalVisitor : public boost::default_astar_visitor
00449             {
00450             public:
00451                 GoalVisitor(const unsigned int goal) : goalRegion(goal)
00452                 {
00453                 }
00454                 void examine_vertex(Vertex v, const RegionGraph& g)
00455                 {
00456                     if (v == goalRegion)
00457                         throw found_goal();
00458                 }
00459             private:
00460                 const unsigned int goalRegion;
00461             };
00463 
00465             class RegionSet
00466             {
00467             public:
00468                 int sampleUniform(void)
00469                 {
00470                     if (empty())
00471                         return -1;
00472                     return regions.sample(rng.uniform01());
00473                 }
00474                 void insert(const int r)
00475                 {
00476                     if (regToElem.count(r) == 0)
00477                         regToElem[r] = regions.add(r, 1);
00478                     else
00479                     {
00480                         PDF<int>::Element* elem = regToElem[r];
00481                         regions.update(elem, regions.getWeight(elem)+1);
00482                     }
00483                 }
00484                 void clear(void)
00485                 {
00486                     regions.clear();
00487                     regToElem.clear();
00488                 }
00489                 std::size_t size(void) const
00490                 {
00491                     return regions.size();
00492                 }
00493                 bool empty() const
00494                 {
00495                     return regions.empty();
00496                 }
00497             private:
00498                 RNG rng;
00499                 PDF<int> regions;
00500                 boost::unordered_map<const int, PDF<int>::Element*> regToElem;
00501             };
00503 
00505             void initRegion(Region& r);
00506 
00508             void setupRegionEstimates(void);
00509 
00511             void updateRegion(Region& r);
00512 
00514             void initEdge(Adjacency& a, const Region* source, const Region* target);
00515 
00517             void setupEdgeEstimates(void);
00518 
00520             void updateEdge(Adjacency& a);
00521 
00524             bool updateCoverageEstimate(Region& r, const base::State* s);
00525 
00528             bool updateConnectionEstimate(const Region& c, const Region& d, const base::State* s);
00529 
00532             void buildGraph(void);
00533 
00535             void clearGraphDetails(void);
00536 
00538             int selectRegion(void);
00539 
00541             void computeAvailableRegions(void);
00542 
00544             void defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead);
00545 
00547             double defaultEdgeCost(int r, int s);
00548 
00550             LeadComputeFn leadComputeFn;
00552             std::vector<int> lead_;
00554             PDF<int> availDist_;
00556             std::vector<EdgeCostFactorFn> edgeCostFactors_;
00558             CoverageGrid covGrid_;
00560             RegionGraph graph_;
00562             bool graphReady_;
00564             boost::unordered_map<std::pair<int,int>, Adjacency*> regionsToEdge_;
00566             unsigned int numMotions_;
00568             RegionSet startRegions_;
00570             RegionSet goalRegions_;
00571         };
00572     }
00573 }
00574 
00575 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines