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