All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
Syclop.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Matt Maly */
36 
37 #include "ompl/control/planners/syclop/Syclop.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/base/ProblemDefinition.h"
40 #include <limits>
41 #include <stack>
42 #include <algorithm>
43 
44 const double ompl::control::Syclop::Defaults::PROB_ABANDON_LEAD_EARLY = 0.25;
45 const double ompl::control::Syclop::Defaults::PROB_KEEP_ADDING_TO_AVAIL = 0.95;
46 const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH = 0.95;
47 
49 {
51  if (!leadComputeFn)
52  setLeadComputeFn(boost::bind(&ompl::control::Syclop::defaultComputeLead, this, _1, _2, _3));
53  buildGraph();
54  addEdgeCostFactor(boost::bind(&ompl::control::Syclop::defaultEdgeCost, this, _1, _2));
55 }
56 
58 {
60  lead_.clear();
61  availDist_.clear();
62  clearGraphDetails();
63  startRegions_.clear();
64  goalRegions_.clear();
65 }
66 
68 {
69  checkValidity();
70  if (!graphReady_)
71  {
72  numMotions_ = 0;
73  setupRegionEstimates();
74  setupEdgeEstimates();
75  graphReady_ = true;
76  }
77  while (const base::State* s = pis_.nextStart())
78  {
79  const int region = decomp_->locateRegion(s);
80  startRegions_.insert(region);
81  Motion* startMotion = addRoot(s);
82  graph_[boost::vertex(region,graph_)].motions.push_back(startMotion);
83  ++numMotions_;
84  updateCoverageEstimate(graph_[boost::vertex(region,graph_)], s);
85  }
86  if (startRegions_.empty())
87  {
88  logError("There are no valid start states");
90  }
91 
92  //We need at least one valid goal sample so that we can find the goal region
93  if (goalRegions_.empty())
94  {
95  if (const base::State* g = pis_.nextGoal(ptc))
96  goalRegions_.insert(decomp_->locateRegion(g));
97  else
98  {
99  logError("Unable to sample a valid goal state");
101  }
102  }
103 
104  logInform("Starting with %u states", numMotions_);
105 
106  std::vector<Motion*> newMotions;
107  const Motion* solution = NULL;
108  base::Goal* goal = pdef_->getGoal().get();
109  double goalDist = std::numeric_limits<double>::infinity();
110  bool solved = false;
111  while (!ptc() && !solved)
112  {
113  const int chosenStartRegion = startRegions_.sampleUniform();
114  int chosenGoalRegion = -1;
115 
116  // if we have not sampled too many goal regions already
117  if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2)
118  {
119  if (const base::State* g = pis_.nextGoal())
120  {
121  chosenGoalRegion = decomp_->locateRegion(g);
122  goalRegions_.insert(chosenGoalRegion);
123  }
124  }
125  if (chosenGoalRegion == -1)
126  chosenGoalRegion = goalRegions_.sampleUniform();
127 
128  leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
129  computeAvailableRegions();
130  for (int i = 0; i < numRegionExpansions_ && !solved && !ptc(); ++i)
131  {
132  const int region = selectRegion();
133  bool improved = false;
134  for (int j = 0; j < numTreeSelections_ && !solved && !ptc(); ++j)
135  {
136  newMotions.clear();
137  selectAndExtend(graph_[boost::vertex(region,graph_)], newMotions);
138  for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc(); ++m)
139  {
140  Motion* motion = *m;
141  double distance;
142  solved = goal->isSatisfied(motion->state, &distance);
143  if (solved)
144  {
145  goalDist = distance;
146  solution = motion;
147  break;
148  }
149 
150  // Check for approximate (best-so-far) solution
151  if (distance < goalDist)
152  {
153  goalDist = distance;
154  solution = motion;
155  }
156  const int newRegion = decomp_->locateRegion(motion->state);
157  graph_[boost::vertex(newRegion,graph_)].motions.push_back(motion);
158  ++numMotions_;
159  Region& newRegionObj = graph_[boost::vertex(newRegion, graph_)];
160  improved |= updateCoverageEstimate(newRegionObj, motion->state);
161  if (newRegion != region)
162  {
163  // If this is the first time the tree has entered this region, add the region to avail
164  if (newRegionObj.motions.size() == 1)
165  availDist_.add(newRegion, newRegionObj.weight);
166  /* If the tree crosses an entire region and creates an edge (u,v) for which Proj(u) and Proj(v) are non-neighboring regions,
167  then we do not update connection estimates. This is because Syclop's shortest-path lead computation only considers neighboring regions. */
168  if (regionsToEdge_.count(std::pair<int,int>(region, newRegion)) > 0)
169  {
170  Adjacency* adj = regionsToEdge_[std::pair<int,int>(region,newRegion)];
171  adj->empty = false;
172  ++adj->numSelections;
173  improved |= updateConnectionEstimate(graph_[boost::vertex(region,graph_)], newRegionObj, motion->state);
174  }
175  }
176  }
177  }
178  if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
179  break;
180  }
181  }
182  bool addedSolution = false;
183  if (solution != NULL)
184  {
185  std::vector<const Motion*> mpath;
186  while (solution != NULL)
187  {
188  mpath.push_back(solution);
189  solution = solution->parent;
190  }
191  PathControl* path = new PathControl(si_);
192  for (int i = mpath.size()-1; i >= 0; --i)
193  if (mpath[i]->parent)
194  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
195  else
196  path->append(mpath[i]->state);
197  pdef_->addSolutionPath(base::PathPtr(path), !solved, goalDist);
198  addedSolution = true;
199  }
201 }
202 
204 {
205  leadComputeFn = compute;
206 }
207 
209 {
210  edgeCostFactors_.push_back(factor);
211 }
212 
214 {
215  edgeCostFactors_.clear();
216 }
217 
218 void ompl::control::Syclop::initRegion(Region& r)
219 {
220  r.numSelections = 0;
221  r.volume = 1.0;
222  r.percentValidCells = 1.0;
223  r.freeVolume = 1.0;
224 }
225 
226 void ompl::control::Syclop::setupRegionEstimates(void)
227 {
228  std::vector<int> numTotal(decomp_->getNumRegions(), 0);
229  std::vector<int> numValid(decomp_->getNumRegions(), 0);
230  base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
231  base::StateSamplerPtr sampler = si_->allocStateSampler();
232  base::State* s = si_->allocState();
233 
234  for (int i = 0; i < numFreeVolSamples_; ++i)
235  {
236  sampler->sampleUniform(s);
237  int rid = decomp_->locateRegion(s);
238  if (checker->isValid(s))
239  ++numValid[rid];
240  ++numTotal[rid];
241  }
242  si_->freeState(s);
243 
244  for (int i = 0; i < decomp_->getNumRegions(); ++i)
245  {
246  Region& r = graph_[boost::vertex(i, graph_)];
247  r.volume = decomp_->getRegionVolume(i);
248  if (numTotal[i] == 0)
249  r.percentValidCells = 1.0;
250  else
251  r.percentValidCells = ((double) numValid[i]) / (double)numTotal[i];
252  r.freeVolume = r.percentValidCells * r.volume;
253  if (r.freeVolume < std::numeric_limits<double>::epsilon())
254  r.freeVolume = std::numeric_limits<double>::epsilon();
255  updateRegion(r);
256  }
257 }
258 
259 void ompl::control::Syclop::updateRegion(Region& r)
260 {
261  const double f = r.freeVolume*r.freeVolume*r.freeVolume*r.freeVolume;
262  r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
263  r.weight = f / ((1 + r.covGridCells.size())*(1 + r.numSelections*r.numSelections));
264 }
265 
266 void ompl::control::Syclop::initEdge(Adjacency& adj, const Region* source, const Region* target)
267 {
268  adj.source = source;
269  adj.target = target;
270  updateEdge(adj);
271  regionsToEdge_[std::pair<int,int>(source->index, target->index)] = &adj;
272 }
273 
274 void ompl::control::Syclop::setupEdgeEstimates(void)
275 {
276  EdgeIter ei, eend;
277  for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
278  {
279  Adjacency& adj = graph_[*ei];
280  adj.empty = true;
281  adj.numLeadInclusions = 0;
282  adj.numSelections = 0;
283  updateEdge(adj);
284  }
285 }
286 
287 void ompl::control::Syclop::updateEdge(Adjacency& a)
288 {
289  a.cost = 1.0;
290  for (std::vector<EdgeCostFactorFn>::const_iterator i = edgeCostFactors_.begin(); i != edgeCostFactors_.end(); ++i)
291  {
292  const EdgeCostFactorFn& factor = *i;
293  a.cost *= factor(a.source->index, a.target->index);
294  }
295 }
296 
297 bool ompl::control::Syclop::updateCoverageEstimate(Region& r, const base::State *s)
298 {
299  const int covCell = covGrid_.locateRegion(s);
300  if (r.covGridCells.count(covCell) == 1)
301  return false;
302  r.covGridCells.insert(covCell);
303  updateRegion(r);
304  return true;
305 }
306 
307 bool ompl::control::Syclop::updateConnectionEstimate(const Region& c, const Region& d, const base::State *s)
308 {
309  Adjacency& adj = *regionsToEdge_[std::pair<int,int>(c.index,d.index)];
310  const int covCell = covGrid_.locateRegion(s);
311  if (adj.covGridCells.count(covCell) == 1)
312  return false;
313  adj.covGridCells.insert(covCell);
314  updateEdge(adj);
315  return true;
316 }
317 
318 void ompl::control::Syclop::buildGraph(void)
319 {
320  VertexIndexMap index = get(boost::vertex_index, graph_);
321  std::vector<int> neighbors;
322  for (int i = 0; i < decomp_->getNumRegions(); ++i)
323  {
324  const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
325  Region& r = graph_[boost::vertex(v,graph_)];
326  initRegion(r);
327  r.index = index[v];
328  }
329  VertexIter vi, vend;
330  for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
331  {
332  /* Create an edge between this vertex and each of its neighboring regions in the decomposition,
333  and initialize the edge's Adjacency object. */
334  decomp_->getNeighbors(index[*vi], neighbors);
335  for (std::vector<int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j)
336  {
337  RegionGraph::edge_descriptor edge;
338  bool ignore;
339  boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(*j,graph_), graph_);
340  initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(*j,graph_)]);
341  }
342  neighbors.clear();
343  }
344 }
345 
346 void ompl::control::Syclop::clearGraphDetails(void)
347 {
348  VertexIter vi, vend;
349  for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
350  graph_[*vi].clear();
351  EdgeIter ei, eend;
352  for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
353  graph_[*ei].clear();
354  graphReady_ = false;
355 }
356 
357 int ompl::control::Syclop::selectRegion(void)
358 {
359  const int index = availDist_.sample(rng_.uniform01());
360  Region& region = graph_[boost::vertex(index,graph_)];
361  ++region.numSelections;
362  updateRegion(region);
363  return index;
364 }
365 
366 void ompl::control::Syclop::computeAvailableRegions(void)
367 {
368  availDist_.clear();
369  for (int i = lead_.size()-1; i >= 0; --i)
370  {
371  Region& r = graph_[boost::vertex(lead_[i],graph_)];
372  if (!r.motions.empty())
373  {
374  availDist_.add(lead_[i], r.weight);
375  if (rng_.uniform01() >= probKeepAddingToAvail_)
376  break;
377  }
378  }
379 }
380 
381 void ompl::control::Syclop::defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead)
382 {
383  lead.clear();
384  if (startRegion == goalRegion)
385  {
386  lead.push_back(startRegion);
387  return;
388  }
389 
390  else if (rng_.uniform01() < probShortestPath_)
391  {
392  std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
393  std::vector<double> distances(decomp_->getNumRegions());
394 
395  try
396  {
397  boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
398  boost::weight_map(get(&Adjacency::cost, graph_)).distance_map(
399  boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)
400  )).predecessor_map(
401  boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))
402  ).visitor(GoalVisitor(goalRegion))
403  );
404  }
405  catch (found_goal fg)
406  {
407  int region = goalRegion;
408  int leadLength = 1;
409 
410  while (region != startRegion)
411  {
412  region = parents[region];
413  ++leadLength;
414  }
415  lead.resize(leadLength);
416  region = goalRegion;
417  for (int i = leadLength-1; i >= 0; --i)
418  {
419  lead[i] = region;
420  region = parents[region];
421  }
422  }
423  }
424  else
425  {
426  /* Run a random-DFS over the decomposition graph from the start region to the goal region.
427  There may be a way to do this using boost::depth_first_search. */
428  VertexIndexMap index = get(boost::vertex_index, graph_);
429  std::stack<int> nodesToProcess;
430  std::vector<int> parents(decomp_->getNumRegions(), -1);
431  parents[startRegion] = startRegion;
432  nodesToProcess.push(startRegion);
433  bool goalFound = false;
434  while (!goalFound && !nodesToProcess.empty())
435  {
436  const int v = nodesToProcess.top();
437  nodesToProcess.pop();
438  std::vector<int> neighbors;
439  boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
440  for (boost::tie(ai,aend) = adjacent_vertices(boost::vertex(v,graph_),graph_); ai != aend; ++ai)
441  {
442  if (parents[index[*ai]] < 0)
443  {
444  neighbors.push_back(index[*ai]);
445  parents[index[*ai]] = v;
446  }
447  }
448  for (std::size_t i = 0; i < neighbors.size(); ++i)
449  {
450  const int choice = rng_.uniformInt(i, neighbors.size()-1);
451  if (neighbors[choice] == goalRegion)
452  {
453  int region = goalRegion;
454  int leadLength = 1;
455  while (region != startRegion)
456  {
457  region = parents[region];
458  ++leadLength;
459  }
460  lead.resize(leadLength);
461  region = goalRegion;
462  for (int j = leadLength-1; j >= 0; --j)
463  {
464  lead[j] = region;
465  region = parents[region];
466  }
467  goalFound = true;
468  break;
469  }
470  nodesToProcess.push(neighbors[choice]);
471  std::swap(neighbors[i], neighbors[choice]);
472  }
473  }
474  }
475 
476  //Now that we have a lead, update the edge weights.
477  for (std::size_t i = 0; i < lead.size()-1; ++i)
478  {
479  Adjacency& adj = *regionsToEdge_[std::pair<int,int>(lead[i], lead[i+1])];
480  if (adj.empty)
481  {
482  ++adj.numLeadInclusions;
483  updateEdge(adj);
484  }
485  }
486 }
487 
488 double ompl::control::Syclop::defaultEdgeCost(int r, int s)
489 {
490  const Adjacency& a = *regionsToEdge_[std::pair<int,int>(r,s)];
491  double factor = 1.0;
492  const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
493  factor = (double)(1 + nsel*nsel) / (double)(1 + a.covGridCells.size()*a.covGridCells.size());
494  factor *= (a.source->alpha * a.target->alpha);
495  return factor;
496 }