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 "ompl/util/DisableCompilerWarning.h"
41 #include <limits>
42 #include <stack>
43 #include <algorithm>
44 
45 const double ompl::control::Syclop::Defaults::PROB_ABANDON_LEAD_EARLY = 0.25;
46 const double ompl::control::Syclop::Defaults::PROB_KEEP_ADDING_TO_AVAIL = 0.50;
47 const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH = 0.95;
48 
50 {
52  if (!leadComputeFn)
53  setLeadComputeFn([this](int startRegion, int goalRegion, std::vector<int> &lead)
54  {
55  defaultComputeLead(startRegion, goalRegion, lead);
56  });
57  buildGraph();
58  addEdgeCostFactor([this](int r, int s)
59  {
60  return defaultEdgeCost(r, s);
61  });
62 }
63 
65 {
67  lead_.clear();
68  availDist_.clear();
69  clearEdgeCostFactors();
70  clearGraphDetails();
71  startRegions_.clear();
72  goalRegions_.clear();
73 }
74 
76 {
77  checkValidity();
78  if (!graphReady_)
79  {
80  numMotions_ = 0;
81  setupRegionEstimates();
82  setupEdgeEstimates();
83  graphReady_ = true;
84  }
85  while (const base::State *s = pis_.nextStart())
86  {
87  const int region = decomp_->locateRegion(s);
88  startRegions_.insert(region);
89  Motion *startMotion = addRoot(s);
90  graph_[boost::vertex(region, graph_)].motions.push_back(startMotion);
91  ++numMotions_;
92  updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s);
93  }
94  if (startRegions_.empty())
95  {
96  OMPL_ERROR("%s: There are no valid start states", getName().c_str());
98  }
99 
100  // We need at least one valid goal sample so that we can find the goal region
101  if (goalRegions_.empty())
102  {
103  if (const base::State *g = pis_.nextGoal(ptc))
104  goalRegions_.insert(decomp_->locateRegion(g));
105  else
106  {
107  OMPL_ERROR("%s: Unable to sample a valid goal state", getName().c_str());
109  }
110  }
111 
112  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
113 
114  std::vector<Motion *> newMotions;
115  const Motion *solution = nullptr;
116  base::Goal *goal = pdef_->getGoal().get();
117  double goalDist = std::numeric_limits<double>::infinity();
118  bool solved = false;
119  while (!ptc && !solved)
120  {
121  const int chosenStartRegion = startRegions_.sampleUniform();
122  int chosenGoalRegion = -1;
123 
124  // if we have not sampled too many goal regions already
125  if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_ / 2)
126  {
127  if (const base::State *g = pis_.nextGoal())
128  {
129  chosenGoalRegion = decomp_->locateRegion(g);
130  goalRegions_.insert(chosenGoalRegion);
131  }
132  }
133  if (chosenGoalRegion == -1)
134  chosenGoalRegion = goalRegions_.sampleUniform();
135 
136  leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
137  computeAvailableRegions();
138  for (int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i)
139  {
140  const int region = selectRegion();
141  bool improved = false;
142  for (int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j)
143  {
144  newMotions.clear();
145  selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions);
146  for (std::vector<Motion *>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
147  {
148  Motion *motion = *m;
149  double distance;
150  solved = goal->isSatisfied(motion->state, &distance);
151  if (solved)
152  {
153  goalDist = distance;
154  solution = motion;
155  break;
156  }
157 
158  // Check for approximate (best-so-far) solution
159  if (distance < goalDist)
160  {
161  goalDist = distance;
162  solution = motion;
163  }
164  const int newRegion = decomp_->locateRegion(motion->state);
165  graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion);
166  ++numMotions_;
167  Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)];
168  improved |= updateCoverageEstimate(newRegionObj, motion->state);
169  /* If tree has just crossed from one region to its neighbor,
170  update the connection estimates. If the tree has crossed an entire region,
171  then region and newRegion are not adjacent, and so we do not update estimates. */
172  if (newRegion != region && regionsToEdge_.count(std::pair<int, int>(region, newRegion)) > 0)
173  {
174  Adjacency *adj = regionsToEdge_[std::pair<int, int>(region, newRegion)];
175  adj->empty = false;
176  ++adj->numSelections;
177  improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj,
178  motion->state);
179  }
180 
181  /* If this region already exists in availDist, update its weight. */
182  if (newRegionObj.pdfElem != nullptr)
183  availDist_.update(newRegionObj.pdfElem, newRegionObj.weight);
184  /* Otherwise, only add this region to availDist
185  if it already exists in the lead. */
186  else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
187  {
188  PDF<int>::Element *elem = availDist_.add(newRegion, newRegionObj.weight);
189  newRegionObj.pdfElem = elem;
190  }
191  }
192  }
193  if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
194  break;
195  }
196  }
197  bool addedSolution = false;
198  if (solution != nullptr)
199  {
200  std::vector<const Motion *> mpath;
201  while (solution != nullptr)
202  {
203  mpath.push_back(solution);
204  solution = solution->parent;
205  }
206  auto path(std::make_shared<PathControl>(si_));
207  for (int i = mpath.size() - 1; i >= 0; --i)
208  if (mpath[i]->parent != nullptr)
209  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
210  else
211  path->append(mpath[i]->state);
212  pdef_->addSolutionPath(path, !solved, goalDist, getName());
213  addedSolution = true;
214  }
216 }
217 
219 {
220  leadComputeFn = compute;
221 }
222 
224 {
225  edgeCostFactors_.push_back(factor);
226 }
227 
229 {
230  edgeCostFactors_.clear();
231 }
232 
233 void ompl::control::Syclop::initRegion(Region &r)
234 {
235  r.numSelections = 0;
236  r.volume = 1.0;
237  r.percentValidCells = 1.0;
238  r.freeVolume = 1.0;
239  r.pdfElem = nullptr;
240 }
241 
242 void ompl::control::Syclop::setupRegionEstimates()
243 {
244  std::vector<int> numTotal(decomp_->getNumRegions(), 0);
245  std::vector<int> numValid(decomp_->getNumRegions(), 0);
246  base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
247  base::StateSamplerPtr sampler = si_->allocStateSampler();
248  base::State *s = si_->allocState();
249 
250  for (int i = 0; i < numFreeVolSamples_; ++i)
251  {
252  sampler->sampleUniform(s);
253  int rid = decomp_->locateRegion(s);
254  if (rid >= 0)
255  {
256  if (checker->isValid(s))
257  ++numValid[rid];
258  ++numTotal[rid];
259  }
260  }
261  si_->freeState(s);
262 
263  for (int i = 0; i < decomp_->getNumRegions(); ++i)
264  {
265  Region &r = graph_[boost::vertex(i, graph_)];
266  r.volume = decomp_->getRegionVolume(i);
267  if (numTotal[i] == 0)
268  r.percentValidCells = 1.0;
269  else
270  r.percentValidCells = ((double)numValid[i]) / (double)numTotal[i];
271  r.freeVolume = r.percentValidCells * r.volume;
272  if (r.freeVolume < std::numeric_limits<double>::epsilon())
273  r.freeVolume = std::numeric_limits<double>::epsilon();
274  updateRegion(r);
275  }
276 }
277 
278 void ompl::control::Syclop::updateRegion(Region &r)
279 {
280  const double f = r.freeVolume * r.freeVolume * r.freeVolume * r.freeVolume;
281  r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
282  r.weight = f / ((1 + r.covGridCells.size()) * (1 + r.numSelections * r.numSelections));
283 }
284 
285 void ompl::control::Syclop::initEdge(Adjacency &adj, const Region *source, const Region *target)
286 {
287  adj.source = source;
288  adj.target = target;
289  updateEdge(adj);
290  regionsToEdge_[std::pair<int, int>(source->index, target->index)] = &adj;
291 }
292 
293 void ompl::control::Syclop::setupEdgeEstimates()
294 {
295 OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
296  EdgeIter ei, eend;
297 OMPL_POP_GCC
298  for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
299  {
300  Adjacency &adj = graph_[*ei];
301  adj.empty = true;
302  adj.numLeadInclusions = 0;
303  adj.numSelections = 0;
304  updateEdge(adj);
305  }
306 }
307 
308 void ompl::control::Syclop::updateEdge(Adjacency &a)
309 {
310  a.cost = 1.0;
311  for (const auto &factor : edgeCostFactors_)
312  {
313  a.cost *= factor(a.source->index, a.target->index);
314  }
315 }
316 
317 bool ompl::control::Syclop::updateCoverageEstimate(Region &r, const base::State *s)
318 {
319  const int covCell = covGrid_.locateRegion(s);
320  if (r.covGridCells.count(covCell) == 1)
321  return false;
322  r.covGridCells.insert(covCell);
323  updateRegion(r);
324  return true;
325 }
326 
327 bool ompl::control::Syclop::updateConnectionEstimate(const Region &c, const Region &d, const base::State *s)
328 {
329  Adjacency &adj = *regionsToEdge_[std::pair<int, int>(c.index, d.index)];
330  const int covCell = covGrid_.locateRegion(s);
331  if (adj.covGridCells.count(covCell) == 1)
332  return false;
333  adj.covGridCells.insert(covCell);
334  updateEdge(adj);
335  return true;
336 }
337 
338 void ompl::control::Syclop::buildGraph()
339 {
340  VertexIndexMap index = get(boost::vertex_index, graph_);
341  std::vector<int> neighbors;
342  for (int i = 0; i < decomp_->getNumRegions(); ++i)
343  {
344  const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
345  Region &r = graph_[boost::vertex(v, graph_)];
346  initRegion(r);
347  r.index = index[v];
348  }
349  VertexIter vi, vend;
350  for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
351  {
352  /* Create an edge between this vertex and each of its neighboring regions in the decomposition,
353  and initialize the edge's Adjacency object. */
354  decomp_->getNeighbors(index[*vi], neighbors);
355  for (const auto &j : neighbors)
356  {
357  RegionGraph::edge_descriptor edge;
358  bool ignore;
359  boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(j, graph_), graph_);
360  initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(j, graph_)]);
361  }
362  neighbors.clear();
363  }
364 }
365 
366 void ompl::control::Syclop::clearGraphDetails()
367 {
368  VertexIter vi, vend;
369  for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
370  graph_[*vi].clear();
371 OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
372  EdgeIter ei, eend;
373 OMPL_POP_GCC
374  for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
375  graph_[*ei].clear();
376  graphReady_ = false;
377 }
378 
379 int ompl::control::Syclop::selectRegion()
380 {
381  const int index = availDist_.sample(rng_.uniform01());
382  Region &region = graph_[boost::vertex(index, graph_)];
383  ++region.numSelections;
384  updateRegion(region);
385  return index;
386 }
387 
388 void ompl::control::Syclop::computeAvailableRegions()
389 {
390  for (unsigned int i = 0; i < availDist_.size(); ++i)
391  graph_[boost::vertex(availDist_[i], graph_)].pdfElem = nullptr;
392  availDist_.clear();
393  for (int i = lead_.size() - 1; i >= 0; --i)
394  {
395  Region &r = graph_[boost::vertex(lead_[i], graph_)];
396  if (!r.motions.empty())
397  {
398  r.pdfElem = availDist_.add(lead_[i], r.weight);
399  if (rng_.uniform01() >= probKeepAddingToAvail_)
400  break;
401  }
402  }
403 }
404 
405 void ompl::control::Syclop::defaultComputeLead(int startRegion, int goalRegion, std::vector<int> &lead)
406 {
407  lead.clear();
408  if (startRegion == goalRegion)
409  {
410  lead.push_back(startRegion);
411  return;
412  }
413 
414  if (rng_.uniform01() < probShortestPath_)
415  {
416  std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
417  std::vector<double> distances(decomp_->getNumRegions());
418 
419  try
420  {
421  boost::astar_search(graph_, boost::vertex(startRegion, graph_),
422  DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
423  boost::weight_map(get(&Adjacency::cost, graph_))
424  .distance_map(boost::make_iterator_property_map(distances.begin(),
425  get(boost::vertex_index, graph_)))
426  .predecessor_map(boost::make_iterator_property_map(
427  parents.begin(), get(boost::vertex_index, graph_)))
428  .visitor(GoalVisitor(goalRegion)));
429  }
430  catch (found_goal fg)
431  {
432  int region = goalRegion;
433  int leadLength = 1;
434 
435  while (region != startRegion)
436  {
437  region = parents[region];
438  ++leadLength;
439  }
440  lead.resize(leadLength);
441  region = goalRegion;
442  for (int i = leadLength - 1; i >= 0; --i)
443  {
444  lead[i] = region;
445  region = parents[region];
446  }
447  }
448  }
449  else
450  {
451  /* Run a random-DFS over the decomposition graph from the start region to the goal region.
452  There may be a way to do this using boost::depth_first_search. */
453  VertexIndexMap index = get(boost::vertex_index, graph_);
454  std::stack<int> nodesToProcess;
455  std::vector<int> parents(decomp_->getNumRegions(), -1);
456  parents[startRegion] = startRegion;
457  nodesToProcess.push(startRegion);
458  bool goalFound = false;
459  while (!goalFound && !nodesToProcess.empty())
460  {
461  const int v = nodesToProcess.top();
462  nodesToProcess.pop();
463  std::vector<int> neighbors;
464  boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
465  for (boost::tie(ai, aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai)
466  {
467  if (parents[index[*ai]] < 0)
468  {
469  neighbors.push_back(index[*ai]);
470  parents[index[*ai]] = v;
471  }
472  }
473  for (std::size_t i = 0; i < neighbors.size(); ++i)
474  {
475  const int choice = rng_.uniformInt(i, neighbors.size() - 1);
476  if (neighbors[choice] == goalRegion)
477  {
478  int region = goalRegion;
479  int leadLength = 1;
480  while (region != startRegion)
481  {
482  region = parents[region];
483  ++leadLength;
484  }
485  lead.resize(leadLength);
486  region = goalRegion;
487  for (int j = leadLength - 1; j >= 0; --j)
488  {
489  lead[j] = region;
490  region = parents[region];
491  }
492  goalFound = true;
493  break;
494  }
495  nodesToProcess.push(neighbors[choice]);
496  std::swap(neighbors[i], neighbors[choice]);
497  }
498  }
499  }
500 
501  // Now that we have a lead, update the edge weights.
502  for (std::size_t i = 0; i < lead.size() - 1; ++i)
503  {
504  Adjacency &adj = *regionsToEdge_[std::pair<int, int>(lead[i], lead[i + 1])];
505  if (adj.empty)
506  {
507  ++adj.numLeadInclusions;
508  updateEdge(adj);
509  }
510  }
511 }
512 
513 double ompl::control::Syclop::defaultEdgeCost(int r, int s)
514 {
515  const Adjacency &a = *regionsToEdge_[std::pair<int, int>(r, s)];
516  double factor = 1.0;
517  const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
518  factor = (1.0 + (double)nsel * nsel) / (1.0 + (double)a.covGridCells.size() * a.covGridCells.size());
519  factor *= (a.source->alpha * a.target->alpha);
520  return factor;
521 }
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element,...
Definition: PDF.h:161
double weight
The probabilistic weight of this region, used when sampling from PDF.
Definition: Syclop.h:404
base::State * state
The state contained by the motion.
Definition: Syclop.h:364
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
Definition: Syclop.h:412
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
Definition: Syclop.cpp:218
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
Definition: Syclop.h:417
Definition of an abstract state.
Definition: State.h:113
bool empty
This value is true if and only if this adjacency's source and target regions both contain zero tree m...
Definition: Syclop.h:443
const Motion * parent
The parent motion in the tree.
Definition: Syclop.h:368
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
Definition: Syclop.h:440
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:80
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:92
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void addEdgeCostFactor(const EdgeCostFactorFn &factor)
Adds an edge cost factor to be used for edge weights between adjacent regions.
Definition: Syclop.cpp:223
@ INVALID_GOAL
Invalid goal state.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:64
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:49
Representation of a motion.
Definition: Syclop.h:352
A class to store the exit status of Planner::solve()
std::function< double(int, int)> EdgeCostFactorFn
Each edge weight between two adjacent regions in the Decomposition is defined as a product of edge co...
Definition: Syclop.h:192
std::function< void(int, int, std::vector< int > &)> LeadComputeFn
Leads should consist of a path of adjacent regions in the decomposition that start with the start reg...
Definition: Syclop.h:196
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:374
Abstract definition of goals.
Definition: Goal.h:126
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:118
@ EXACT_SOLUTION
The planner found an exact solution.
void clearEdgeCostFactors()
Clears all edge cost factors, making all edge weights equivalent to 1.
Definition: Syclop.cpp:228
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continues solving until a solution is found or a given planner termination condition is met....
Definition: Syclop.cpp:75
@ INVALID_START
Invalid start state or no start state specified.