All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
Syclop.cpp
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 #include "ompl/control/planners/syclop/Syclop.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/base/ProblemDefinition.h"
00040 #include <limits>
00041 #include <stack>
00042 #include <algorithm>
00043 
00044 const double ompl::control::Syclop::Defaults::PROB_ABANDON_LEAD_EARLY   = 0.25;
00045 const double ompl::control::Syclop::Defaults::PROB_KEEP_ADDING_TO_AVAIL = 0.95;
00046 const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH        = 0.95;
00047 
00048 void ompl::control::Syclop::setup(void)
00049 {
00050     base::Planner::setup();
00051     buildGraph();
00052     addEdgeCostFactor(boost::bind(&ompl::control::Syclop::defaultEdgeCost, this, _1, _2));
00053 }
00054 
00055 void ompl::control::Syclop::clear(void)
00056 {
00057     base::Planner::clear();
00058     lead_.clear();
00059     availDist_.clear();
00060     clearGraphDetails();
00061     startRegions_.clear();
00062     goalRegions_.clear();
00063 }
00064 
00065 bool ompl::control::Syclop::solve(const base::PlannerTerminationCondition& ptc)
00066 {
00067     checkValidity();
00068     if (!graphReady_)
00069     {
00070         numMotions_ = 0;
00071         setupRegionEstimates();
00072         setupEdgeEstimates();
00073         graphReady_ = true;
00074     }
00075     while (const base::State* s = pis_.nextStart())
00076     {
00077         const int region = decomp_->locateRegion(s);
00078         startRegions_.insert(region);
00079         Motion* startMotion = addRoot(s);
00080         graph_[boost::vertex(region,graph_)].motions.push_back(startMotion);
00081         ++numMotions_;
00082         updateCoverageEstimate(graph_[boost::vertex(region,graph_)], s);
00083     }
00084     if (startRegions_.empty())
00085     {
00086         msg_.error("There are no valid start states");
00087         return false;
00088     }
00089 
00090     //We need at least one valid goal sample so that we can find the goal region
00091     if (goalRegions_.empty())
00092     {
00093         if (const base::State* g = pis_.nextGoal(ptc))
00094             goalRegions_.insert(decomp_->locateRegion(g));
00095         else
00096         {
00097             msg_.error("Unable to sample a valid goal state");
00098             return false;
00099         }
00100     }
00101 
00102     msg_.inform("Starting with %u states", numMotions_);
00103 
00104     std::vector<Motion*> newMotions;
00105     const Motion* solution = NULL;
00106     base::Goal* goal = pdef_->getGoal().get();
00107     double goalDist = std::numeric_limits<double>::infinity();
00108     bool solved = false;
00109     while (!ptc() && !solved)
00110     {
00111         const int chosenStartRegion = startRegions_.sampleUniform();
00112         int chosenGoalRegion = -1;
00113 
00114         // if we have not sampled too many goal regions already
00115         if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2)
00116         {
00117             if (const base::State* g = pis_.nextGoal())
00118             {
00119                 chosenGoalRegion = decomp_->locateRegion(g);
00120                 goalRegions_.insert(chosenGoalRegion);
00121             }
00122         }
00123         if (chosenGoalRegion == -1)
00124             chosenGoalRegion = goalRegions_.sampleUniform();
00125 
00126         computeLead(chosenStartRegion, chosenGoalRegion);
00127         computeAvailableRegions();
00128         for (int i = 0; i < numRegionExpansions_ && !solved && !ptc(); ++i)
00129         {
00130             const int region = selectRegion();
00131             bool improved = false;
00132             for (int j = 0; j < numTreeSelections_ && !solved && !ptc(); ++j)
00133             {
00134                 newMotions.clear();
00135                 selectAndExtend(graph_[boost::vertex(region,graph_)], newMotions);
00136                 for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc(); ++m)
00137                 {
00138                     Motion* motion = *m;
00139                     double distance;
00140                     solved = goal->isSatisfied(motion->state, &distance);
00141                     if (solved)
00142                     {
00143                         goalDist = distance;
00144                         solution = motion;
00145                         break;
00146                     }
00147 
00148                     // Check for approximate (best-so-far) solution
00149                     if (distance < goalDist)
00150                     {
00151                         goalDist = distance;
00152                         solution = motion;
00153                     }
00154                     const int newRegion = decomp_->locateRegion(motion->state);
00155                     graph_[boost::vertex(newRegion,graph_)].motions.push_back(motion);
00156                     ++numMotions_;
00157                     Region& newRegionObj = graph_[boost::vertex(newRegion, graph_)];
00158                     improved |= updateCoverageEstimate(newRegionObj, motion->state);
00159                     if (newRegion != region)
00160                     {
00161                         // If this is the first time the tree has entered this region, add the region to avail
00162                         if (newRegionObj.motions.size() == 1)
00163                             availDist_.add(newRegion, newRegionObj.weight);
00164                         /* If the tree crosses an entire region and creates an edge (u,v) for which Proj(u) and Proj(v) are non-neighboring regions,
00165                             then we do not update connection estimates. This is because Syclop's shortest-path lead computation only considers neighboring regions. */
00166                         if (regionsToEdge_.count(std::pair<int,int>(region, newRegion)) > 0)
00167                         {
00168                             Adjacency* adj = regionsToEdge_[std::pair<int,int>(region,newRegion)];
00169                             adj->empty = false;
00170                             ++adj->numSelections;
00171                             improved |= updateConnectionEstimate(graph_[boost::vertex(region,graph_)], newRegionObj, motion->state);
00172                         }
00173                     }
00174                 }
00175             }
00176             if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
00177                 break;
00178         }
00179     }
00180     bool addedSolution = false;
00181     if (solution != NULL)
00182     {
00183         std::vector<const Motion*> mpath;
00184         while (solution != NULL)
00185         {
00186             mpath.push_back(solution);
00187             solution = solution->parent;
00188         }
00189         PathControl* path = new PathControl(si_);
00190         for (int i = mpath.size()-1; i >= 0; --i)
00191             if (mpath[i]->parent)
00192                 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
00193             else
00194                 path->append(mpath[i]->state);
00195         goal->addSolutionPath(base::PathPtr(path), !solved, goalDist);
00196         addedSolution = true;
00197     }
00198     return addedSolution;
00199 }
00200 
00201 void ompl::control::Syclop::addEdgeCostFactor(const EdgeCostFactorFn& factor)
00202 {
00203     edgeCostFactors_.push_back(factor);
00204 }
00205 
00206 void ompl::control::Syclop::clearEdgeCostFactors(void)
00207 {
00208     edgeCostFactors_.clear();
00209 }
00210 
00211 void ompl::control::Syclop::initRegion(Region& r)
00212 {
00213     r.numSelections = 0;
00214     r.volume = 1.0;
00215     r.percentValidCells = 1.0;
00216     r.freeVolume = 1.0;
00217 }
00218 
00219 void ompl::control::Syclop::setupRegionEstimates(void)
00220 {
00221     std::vector<int> numTotal(decomp_->getNumRegions(), 0);
00222     std::vector<int> numValid(decomp_->getNumRegions(), 0);
00223     base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
00224     base::StateSamplerPtr sampler = si_->allocStateSampler();
00225     base::State* s = si_->allocState();
00226 
00227     for (int i = 0; i < numFreeVolSamples_; ++i)
00228     {
00229         sampler->sampleUniform(s);
00230         int rid = decomp_->locateRegion(s);
00231         if (checker->isValid(s))
00232             ++numValid[rid];
00233         ++numTotal[rid];
00234     }
00235     si_->freeState(s);
00236 
00237     for (int i = 0; i < decomp_->getNumRegions(); ++i)
00238     {
00239         Region& r = graph_[boost::vertex(i, graph_)];
00240         r.volume = decomp_->getRegionVolume(i);
00241         if (numTotal[i] == 0)
00242             r.percentValidCells = 1.0;
00243         else
00244             r.percentValidCells = ((double) numValid[i]) / (double)numTotal[i];
00245         r.freeVolume = r.percentValidCells * r.volume;
00246         if (r.freeVolume < std::numeric_limits<double>::epsilon())
00247             r.freeVolume = std::numeric_limits<double>::epsilon();
00248         updateRegion(r);
00249     }
00250 }
00251 
00252 void ompl::control::Syclop::updateRegion(Region& r)
00253 {
00254     const double f = r.freeVolume*r.freeVolume*r.freeVolume*r.freeVolume;
00255     r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
00256     r.weight = f / ((1 + r.covGridCells.size())*(1 + r.numSelections*r.numSelections));
00257 }
00258 
00259 void ompl::control::Syclop::initEdge(Adjacency& adj, const Region* source, const Region* target)
00260 {
00261     adj.source = source;
00262     adj.target = target;
00263     updateEdge(adj);
00264     regionsToEdge_[std::pair<int,int>(source->index, target->index)] = &adj;
00265 }
00266 
00267 void ompl::control::Syclop::setupEdgeEstimates(void)
00268 {
00269     EdgeIter ei, eend;
00270     for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
00271     {
00272         Adjacency& adj = graph_[*ei];
00273         adj.empty = true;
00274         adj.numLeadInclusions = 0;
00275         adj.numSelections = 0;
00276         updateEdge(adj);
00277     }
00278 }
00279 
00280 void ompl::control::Syclop::updateEdge(Adjacency& a)
00281 {
00282     a.cost = 1.0;
00283     for (std::vector<EdgeCostFactorFn>::const_iterator i = edgeCostFactors_.begin(); i != edgeCostFactors_.end(); ++i)
00284     {
00285         const EdgeCostFactorFn& factor = *i;
00286         a.cost *= factor(a.source->index, a.target->index);
00287     }
00288 }
00289 
00290 bool ompl::control::Syclop::updateCoverageEstimate(Region& r, const base::State *s)
00291 {
00292     const int covCell = covGrid_.locateRegion(s);
00293     if (r.covGridCells.count(covCell) == 1)
00294         return false;
00295     r.covGridCells.insert(covCell);
00296     updateRegion(r);
00297     return true;
00298 }
00299 
00300 bool ompl::control::Syclop::updateConnectionEstimate(const Region& c, const Region& d, const base::State *s)
00301 {
00302     Adjacency& adj = *regionsToEdge_[std::pair<int,int>(c.index,d.index)];
00303     const int covCell = covGrid_.locateRegion(s);
00304     if (adj.covGridCells.count(covCell) == 1)
00305         return false;
00306     adj.covGridCells.insert(covCell);
00307     updateEdge(adj);
00308     return true;
00309 }
00310 
00311 void ompl::control::Syclop::buildGraph(void)
00312 {
00313     VertexIndexMap index = get(boost::vertex_index, graph_);
00314     std::vector<int> neighbors;
00315     for (int i = 0; i < decomp_->getNumRegions(); ++i)
00316     {
00317         const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
00318         Region& r = graph_[boost::vertex(v,graph_)];
00319         initRegion(r);
00320         r.index = index[v];
00321     }
00322     VertexIter vi, vend;
00323     for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
00324     {
00325         /* Create an edge between this vertex and each of its neighboring regions in the decomposition,
00326             and initialize the edge's Adjacency object. */
00327         decomp_->getNeighbors(index[*vi], neighbors);
00328         for (std::vector<int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j)
00329         {
00330             RegionGraph::edge_descriptor edge;
00331             bool ignore;
00332             boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(*j,graph_), graph_);
00333             initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(*j,graph_)]);
00334         }
00335         neighbors.clear();
00336     }
00337 }
00338 
00339 void ompl::control::Syclop::clearGraphDetails(void)
00340 {
00341     VertexIter vi, vend;
00342     for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
00343         graph_[*vi].clear();
00344     EdgeIter ei, eend;
00345     for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
00346         graph_[*ei].clear();
00347     graphReady_ = false;
00348 }
00349 
00350 void ompl::control::Syclop::computeLead(int startRegion, int goalRegion)
00351 {
00352     lead_.clear();
00353     if (startRegion == goalRegion)
00354     {
00355         lead_.push_back(startRegion);
00356         return;
00357     }
00358 
00359     else if (rng_.uniform01() < probShortestPath_)
00360     {
00361         std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
00362         std::vector<double> distances(decomp_->getNumRegions());
00363 
00364         try
00365         {
00366             boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
00367                 boost::weight_map(get(&Adjacency::cost, graph_)).distance_map(
00368                     boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)
00369                 )).predecessor_map(
00370                     boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))
00371                 ).visitor(GoalVisitor(goalRegion))
00372             );
00373         }
00374         catch (found_goal fg)
00375         {
00376             int region = goalRegion;
00377             int leadLength = 1;
00378 
00379             while (region != startRegion)
00380             {
00381                 region = parents[region];
00382                 ++leadLength;
00383             }
00384             lead_.resize(leadLength);
00385             region = goalRegion;
00386             for (int i = leadLength-1; i >= 0; --i)
00387             {
00388                 lead_[i] = region;
00389                 region = parents[region];
00390             }
00391         }
00392     }
00393     else
00394     {
00395         /* Run a random-DFS over the decomposition graph from the start region to the goal region.
00396            There may be a way to do this using boost::depth_first_search. */
00397         VertexIndexMap index = get(boost::vertex_index, graph_);
00398         std::stack<int> nodesToProcess;
00399         std::vector<int> parents(decomp_->getNumRegions(), -1);
00400         parents[startRegion] = startRegion;
00401         nodesToProcess.push(startRegion);
00402         bool goalFound = false;
00403         while (!goalFound && !nodesToProcess.empty())
00404         {
00405             const int v = nodesToProcess.top();
00406             nodesToProcess.pop();
00407             std::vector<int> neighbors;
00408             boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
00409             for (boost::tie(ai,aend) = adjacent_vertices(boost::vertex(v,graph_),graph_); ai != aend; ++ai)
00410             {
00411                 if (parents[index[*ai]] < 0)
00412                 {
00413                     neighbors.push_back(index[*ai]);
00414                     parents[index[*ai]] = v;
00415                 }
00416             }
00417             for (std::size_t i = 0; i < neighbors.size(); ++i)
00418             {
00419                 const int choice = rng_.uniformInt(i, neighbors.size()-1);
00420                 if (neighbors[choice] == goalRegion)
00421                 {
00422                     int region = goalRegion;
00423                     int leadLength = 1;
00424                     while (region != startRegion)
00425                     {
00426                         region = parents[region];
00427                         ++leadLength;
00428                     }
00429                     lead_.resize(leadLength);
00430                     region = goalRegion;
00431                     for (int j = leadLength-1; j >= 0; --j)
00432                     {
00433                         lead_[j] = region;
00434                         region = parents[region];
00435                     }
00436                     goalFound = true;
00437                     break;
00438                 }
00439                 nodesToProcess.push(neighbors[choice]);
00440                 std::swap(neighbors[i], neighbors[choice]);
00441             }
00442         }
00443     }
00444 
00445     //Now that we have a lead, update the edge weights.
00446     for (std::size_t i = 0; i < lead_.size()-1; ++i)
00447     {
00448         Adjacency& adj = *regionsToEdge_[std::pair<int,int>(lead_[i], lead_[i+1])];
00449         if (adj.empty)
00450         {
00451             ++adj.numLeadInclusions;
00452             updateEdge(adj);
00453         }
00454     }
00455 }
00456 
00457 int ompl::control::Syclop::selectRegion(void)
00458 {
00459     const int index = availDist_.sample(rng_.uniform01());
00460     Region& region = graph_[boost::vertex(index,graph_)];
00461     ++region.numSelections;
00462     updateRegion(region);
00463     return index;
00464 }
00465 
00466 void ompl::control::Syclop::computeAvailableRegions(void)
00467 {
00468     availDist_.clear();
00469     for (int i = lead_.size()-1; i >= 0; --i)
00470     {
00471         Region& r = graph_[boost::vertex(lead_[i],graph_)];
00472         if (!r.motions.empty())
00473         {
00474             availDist_.add(lead_[i], r.weight);
00475             if (rng_.uniform01() >= probKeepAddingToAvail_)
00476                 break;
00477         }
00478     }
00479 }
00480 
00481 double ompl::control::Syclop::defaultEdgeCost(int r, int s)
00482 {
00483     const Adjacency& a = *regionsToEdge_[std::pair<int,int>(r,s)];
00484     double factor = 1.0;
00485     const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
00486     factor = (double)(1 + nsel*nsel) / (double)(1 + a.covGridCells.size()*a.covGridCells.size());
00487     factor *= (a.source->alpha * a.target->alpha);
00488     return factor;
00489 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends