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 {
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
00233
00234
00235 static const double PROB_ABANDON_LEAD_EARLY ;
00236 static const double PROB_KEEP_ADDING_TO_AVAIL ;
00237 static const double PROB_SHORTEST_PATH ;
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