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 ®ion, 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 ®ion = 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