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