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