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