00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00237
00238
00239 static const double PROB_ABANDON_LEAD_EARLY ;
00240 static const double PROB_KEEP_ADDING_TO_AVAIL ;
00241 static const double PROB_SHORTEST_PATH ;
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