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, "10000:10000:500000");
00110                 Planner::declareParam<int>   ("num_region_expansions", this, &Syclop::setNumRegionExpansions, &Syclop::getNumRegionExpansions, "10:10:500");
00111                 Planner::declareParam<int>   ("num_tree_expansions", this, &Syclop::setNumTreeExpansions, &Syclop::getNumTreeExpansions, "0:1:100");
00112                 Planner::declareParam<double>("prob_abandon_lead_early", this, &Syclop::setProbAbandonLeadEarly, &Syclop::getProbAbandonLeadEarly, "0.:.05:1.");
00113                 Planner::declareParam<double>("prob_add_available_regions", this, &Syclop::setProbAddingToAvailableRegions, &Syclop::getProbAddingToAvailableRegions, "0.:.05:1.");
00114                 Planner::declareParam<double>("prob_shortest_path_lead", this, &Syclop::setProbShortestPathLead, &Syclop::getProbShortestPathLead, "0.:.05:1.");
00115             }
00116 
00117             virtual ~Syclop()
00118             {
00119             }
00120 
00123 
00124             virtual void setup();
00125 
00126             virtual void clear();
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();
00144 
00146             int getNumFreeVolumeSamples () const
00147             {
00148                 return numFreeVolSamples_;
00149             }
00150 
00153             void setNumFreeVolumeSamples (int numSamples)
00154             {
00155                 numFreeVolSamples_ = numSamples;
00156             }
00157 
00160             double getProbShortestPathLead () const
00161             {
00162                 return probShortestPath_;
00163             }
00164 
00167             void setProbShortestPathLead (double probability)
00168             {
00169                 probShortestPath_ = probability;
00170             }
00171 
00174             double getProbAddingToAvailableRegions () const
00175             {
00176                 return probKeepAddingToAvail_;
00177             }
00178 
00181             void setProbAddingToAvailableRegions (double probability)
00182             {
00183                 probKeepAddingToAvail_ = probability;
00184             }
00185 
00188             int getNumRegionExpansions () const
00189             {
00190                 return numRegionExpansions_;
00191             }
00192 
00195             void setNumRegionExpansions (int regionExpansions)
00196             {
00197                 numRegionExpansions_ = regionExpansions;
00198             }
00199 
00202             int getNumTreeExpansions () const
00203             {
00204                 return numTreeSelections_;
00205             }
00206 
00209             void setNumTreeExpansions (int treeExpansions)
00210             {
00211                 numTreeSelections_ = treeExpansions;
00212             }
00213 
00216             double getProbAbandonLeadEarly () 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         = 1;
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.50*/;
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() : 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()
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()
00281                 {
00282                 }
00283                 virtual ~Region()
00284                 {
00285                 }
00287                 void clear()
00288                 {
00289                     motions.clear();
00290                     covGridCells.clear();
00291                     pdfElem = NULL;
00292                 }
00293 
00295                 std::set<int> covGridCells;
00297                 std::vector<Motion*> motions;
00299                 double volume;
00301                 double freeVolume;
00303                 double percentValidCells;
00305                 double weight;
00307                 double alpha;
00309                 int index;
00311                 unsigned int numSelections;
00313                 PDF<int>::Element *pdfElem;
00314             };
00315             #pragma pack (pop)  // Restoring default byte alignment
00316 
00317             #pragma pack(push, 4)  // push default byte alignment to stack and align the following structure to 4 byte boundary
00318 
00320             class Adjacency
00321             {
00322             public:
00323                 Adjacency()
00324                 {
00325                 }
00326                 virtual ~Adjacency()
00327                 {
00328                 }
00330                 void clear()
00331                 {
00332                     covGridCells.clear();
00333                 }
00336                 std::set<int> covGridCells;
00338                 const Region *source;
00340                 const Region *target;
00342                 double cost;
00344                 int numLeadInclusions;
00347                 int numSelections;
00349                 bool empty;
00350             };
00351             #pragma pack (pop) // Restoring default byte alignment
00352 
00354             virtual Motion* addRoot(const base::State *s) = 0;
00355 
00358             virtual void selectAndExtend(Region &region, std::vector<Motion*>& newMotions) = 0;
00359 
00361             inline const Region& getRegionFromIndex(const int rid) const
00362             {
00363                 return graph_[boost::vertex(rid,graph_)];
00364             }
00365 
00367             int numFreeVolSamples_;
00368 
00370             double probShortestPath_;
00371 
00373             double probKeepAddingToAvail_;
00374 
00376             int numRegionExpansions_;
00377 
00379             int numTreeSelections_;
00380 
00382             double probAbandonLeadEarly_;
00383 
00385             const SpaceInformation *siC_;
00386 
00388             DecompositionPtr decomp_;
00389 
00391             RNG rng_;
00392 
00393         private:
00396             class CoverageGrid : public GridDecomposition
00397             {
00398             public:
00399                 CoverageGrid(const int len, const DecompositionPtr& d) : GridDecomposition(len, d->getDimension(), d->getBounds()), decomp(d)
00400                 {
00401                 }
00402 
00403                 virtual ~CoverageGrid()
00404                 {
00405                 }
00406 
00409                 virtual void project(const base::State *s, std::vector<double>& coord) const
00410                 {
00411                     decomp->project(s, coord);
00412                 }
00413 
00415                 virtual void sampleFullState(const base::StateSamplerPtr& /*sampler*/, const std::vector<double>& /*coord*/, base::State* /*s*/) const
00416                 {
00417                 }
00418 
00419             protected:
00420                 const DecompositionPtr& decomp;
00421             };
00422 
00423             typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
00424             typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
00425             typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
00426             typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
00427             typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;
00428 
00430             friend class DecompositionHeuristic;
00431 
00432             class DecompositionHeuristic : public boost::astar_heuristic<RegionGraph, double>
00433             {
00434             public:
00435                 DecompositionHeuristic(const Syclop *s, const Region &goal) : syclop(s), goalRegion(goal)
00436                 {
00437                 }
00438 
00439                 double operator()(Vertex v)
00440                 {
00441                     const Region &region = syclop->getRegionFromIndex(v);
00442                     return region.alpha*goalRegion.alpha;
00443                 }
00444             private:
00445                 const Syclop *syclop;
00446                 const Region &goalRegion;
00447             };
00448 
00449             struct found_goal {};
00450 
00451             class GoalVisitor : public boost::default_astar_visitor
00452             {
00453             public:
00454                 GoalVisitor(const int goal) : goalRegion(goal)
00455                 {
00456                 }
00457                 void examine_vertex(Vertex v, const RegionGraph& /*g*/)
00458                 {
00459                     if (static_cast<int>(v) == goalRegion)
00460                         throw found_goal();
00461                 }
00462             private:
00463                 const int goalRegion;
00464             };
00466 
00468             class RegionSet
00469             {
00470             public:
00471                 int sampleUniform()
00472                 {
00473                     if (empty())
00474                         return -1;
00475                     return regions.sample(rng.uniform01());
00476                 }
00477                 void insert(const int r)
00478                 {
00479                     if (regToElem.count(r) == 0)
00480                         regToElem[r] = regions.add(r, 1);
00481                     else
00482                     {
00483                         PDF<int>::Element *elem = regToElem[r];
00484                         regions.update(elem, regions.getWeight(elem)+1);
00485                     }
00486                 }
00487                 void clear()
00488                 {
00489                     regions.clear();
00490                     regToElem.clear();
00491                 }
00492                 std::size_t size() const
00493                 {
00494                     return regions.size();
00495                 }
00496                 bool empty() const
00497                 {
00498                     return regions.empty();
00499                 }
00500             private:
00501                 RNG rng;
00502                 PDF<int> regions;
00503                 boost::unordered_map<const int, PDF<int>::Element*> regToElem;
00504             };
00506 
00508             void initRegion(Region &r);
00509 
00511             void setupRegionEstimates();
00512 
00514             void updateRegion(Region &r);
00515 
00517             void initEdge(Adjacency &a, const Region *source, const Region *target);
00518 
00520             void setupEdgeEstimates();
00521 
00523             void updateEdge(Adjacency &a);
00524 
00527             bool updateCoverageEstimate(Region &r, const base::State *s);
00528 
00531             bool updateConnectionEstimate(const Region &c, const Region &d, const base::State *s);
00532 
00535             void buildGraph();
00536 
00538             void clearGraphDetails();
00539 
00541             int selectRegion();
00542 
00544             void computeAvailableRegions();
00545 
00547             void defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead);
00548 
00550             double defaultEdgeCost(int r, int s);
00551 
00553             LeadComputeFn leadComputeFn;
00555             std::vector<int> lead_;
00557             PDF<int> availDist_;
00559             std::vector<EdgeCostFactorFn> edgeCostFactors_;
00561             CoverageGrid covGrid_;
00563             RegionGraph graph_;
00565             bool graphReady_;
00567             boost::unordered_map<std::pair<int,int>, Adjacency*> regionsToEdge_;
00569             unsigned int numMotions_;
00571             RegionSet startRegions_;
00573             RegionSet goalRegions_;
00574         };
00575     }
00576 }
00577 
00578 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines