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.50;
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  OMPL_ERROR("%s: There are no valid start states", getName().c_str());
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  OMPL_ERROR("%s: Unable to sample a valid goal state", getName().c_str());
101  }
102  }
103 
104  OMPL_INFORM("%s: Starting with %u states", getName().c_str(), 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 tree has just crossed from one region to its neighbor,
162  update the connection estimates. If the tree has crossed an entire region,
163  then region and newRegion are not adjacent, and so we do not update estimates. */
164  if (newRegion != region
165  && regionsToEdge_.count(std::pair<int,int>(region,newRegion)) > 0)
166  {
167  Adjacency* adj = regionsToEdge_[std::pair<int,int>(region,newRegion)];
168  adj->empty = false;
169  ++adj->numSelections;
170  improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj, motion->state);
171  }
172 
173  /* If this region already exists in availDist, update its weight. */
174  if (newRegionObj.pdfElem != NULL)
175  availDist_.update(newRegionObj.pdfElem, newRegionObj.weight);
176  /* Otherwise, only add this region to availDist
177  if it already exists in the lead. */
178  else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
179  {
180  PDF<int>::Element* elem = availDist_.add(newRegion, newRegionObj.weight);
181  newRegionObj.pdfElem = elem;
182  }
183  }
184  }
185  if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
186  break;
187  }
188  }
189  bool addedSolution = false;
190  if (solution != NULL)
191  {
192  std::vector<const Motion*> mpath;
193  while (solution != NULL)
194  {
195  mpath.push_back(solution);
196  solution = solution->parent;
197  }
198  PathControl* path = new PathControl(si_);
199  for (int i = mpath.size()-1; i >= 0; --i)
200  if (mpath[i]->parent)
201  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
202  else
203  path->append(mpath[i]->state);
204  pdef_->addSolutionPath(base::PathPtr(path), !solved, goalDist);
205  addedSolution = true;
206  }
208 }
209 
211 {
212  leadComputeFn = compute;
213 }
214 
216 {
217  edgeCostFactors_.push_back(factor);
218 }
219 
221 {
222  edgeCostFactors_.clear();
223 }
224 
225 void ompl::control::Syclop::initRegion(Region& r)
226 {
227  r.numSelections = 0;
228  r.volume = 1.0;
229  r.percentValidCells = 1.0;
230  r.freeVolume = 1.0;
231  r.pdfElem = NULL;
232 }
233 
234 void ompl::control::Syclop::setupRegionEstimates(void)
235 {
236  std::vector<int> numTotal(decomp_->getNumRegions(), 0);
237  std::vector<int> numValid(decomp_->getNumRegions(), 0);
238  base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
239  base::StateSamplerPtr sampler = si_->allocStateSampler();
240  base::State* s = si_->allocState();
241 
242  for (int i = 0; i < numFreeVolSamples_; ++i)
243  {
244  sampler->sampleUniform(s);
245  int rid = decomp_->locateRegion(s);
246  if (rid >= 0)
247  {
248  if (checker->isValid(s))
249  ++numValid[rid];
250  ++numTotal[rid];
251  }
252  }
253  si_->freeState(s);
254 
255  for (unsigned int i = 0; i < decomp_->getNumRegions(); ++i)
256  {
257  Region& r = graph_[boost::vertex(i, graph_)];
258  r.volume = decomp_->getRegionVolume(i);
259  if (numTotal[i] == 0)
260  r.percentValidCells = 1.0;
261  else
262  r.percentValidCells = ((double) numValid[i]) / (double)numTotal[i];
263  r.freeVolume = r.percentValidCells * r.volume;
264  if (r.freeVolume < std::numeric_limits<double>::epsilon())
265  r.freeVolume = std::numeric_limits<double>::epsilon();
266  updateRegion(r);
267  }
268 }
269 
270 void ompl::control::Syclop::updateRegion(Region& r)
271 {
272  const double f = r.freeVolume*r.freeVolume*r.freeVolume*r.freeVolume;
273  r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
274  r.weight = f / ((1 + r.covGridCells.size())*(1 + r.numSelections*r.numSelections));
275 }
276 
277 void ompl::control::Syclop::initEdge(Adjacency& adj, const Region* source, const Region* target)
278 {
279  adj.source = source;
280  adj.target = target;
281  updateEdge(adj);
282  regionsToEdge_[std::pair<int,int>(source->index, target->index)] = &adj;
283 }
284 
285 void ompl::control::Syclop::setupEdgeEstimates(void)
286 {
287  EdgeIter ei, eend;
288  for (boost::tie(ei,eend) = boost::edges(graph_) ; ei != eend; ++ei)
289  {
290  Adjacency& adj = graph_[*ei];
291  adj.empty = true;
292  adj.numLeadInclusions = 0;
293  adj.numSelections = 0;
294  updateEdge(adj);
295  }
296 }
297 
298 void ompl::control::Syclop::updateEdge(Adjacency& a)
299 {
300  a.cost = 1.0;
301  for (std::vector<EdgeCostFactorFn>::const_iterator i = edgeCostFactors_.begin(); i != edgeCostFactors_.end(); ++i)
302  {
303  const EdgeCostFactorFn& factor = *i;
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(void)
330 {
331  VertexIndexMap index = get(boost::vertex_index, graph_);
332  std::vector<unsigned int> neighbors;
333  for (unsigned 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 (std::vector<unsigned int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j)
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(void)
358 {
359  VertexIter vi, vend;
360  for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
361  graph_[*vi].clear();
362  EdgeIter ei, eend;
363  for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
364  graph_[*ei].clear();
365  graphReady_ = false;
366 }
367 
368 int ompl::control::Syclop::selectRegion(void)
369 {
370  const int index = availDist_.sample(rng_.uniform01());
371  Region& region = graph_[boost::vertex(index, graph_)];
372  ++region.numSelections;
373  updateRegion(region);
374  return index;
375 }
376 
377 void ompl::control::Syclop::computeAvailableRegions(void)
378 {
379  for (unsigned int i = 0; i < availDist_.size(); ++i)
380  graph_[boost::vertex(availDist_[i],graph_)].pdfElem = NULL;
381  availDist_.clear();
382  for (int i = lead_.size()-1; i >= 0; --i)
383  {
384  Region& r = graph_[boost::vertex(lead_[i], graph_)];
385  if (!r.motions.empty())
386  {
387  r.pdfElem = availDist_.add(lead_[i], r.weight);
388  if (rng_.uniform01() >= probKeepAddingToAvail_)
389  break;
390  }
391  }
392 }
393 
394 void ompl::control::Syclop::defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead)
395 {
396  lead.clear();
397  if (startRegion == goalRegion)
398  {
399  lead.push_back(startRegion);
400  return;
401  }
402 
403  else if (rng_.uniform01() < probShortestPath_)
404  {
405  std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
406  std::vector<double> distances(decomp_->getNumRegions());
407 
408  try
409  {
410  boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
411  boost::weight_map(get(&Adjacency::cost, graph_)).distance_map(
412  boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)
413  )).predecessor_map(
414  boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))
415  ).visitor(GoalVisitor(goalRegion))
416  );
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 = (double)(1 + nsel*nsel) / (double)(1 + a.covGridCells.size()*a.covGridCells.size());
507  factor *= (a.source->alpha * a.target->alpha);
508  return factor;
509 }
base::State * state
The state contained by the motion.
Definition: Syclop.h:265
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
Definition: Syclop.h:319
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
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:346
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
Definition: Syclop.h:312
A boost shared pointer wrapper for ompl::base::StateSampler.
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:277
Abstract definition of goals.
Definition: Goal.h:63
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Representation of a motion.
Definition: Syclop.h:251
Definition of a control path.
Definition: PathControl.h:54
void clearEdgeCostFactors(void)
Clears all edge cost factors, making all edge weights equivalent to 1.
Definition: Syclop.cpp:220
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:48
boost::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:91
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continues solving until a solution is found or a given planner termination condition is met...
Definition: Syclop.cpp:67
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:57
void addEdgeCostFactor(const EdgeCostFactorFn &factor)
Adds an edge cost factor to be used for edge weights between adjacent regions.
Definition: Syclop.cpp:215
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:348
The planner found an exact solution.
Definition: PlannerStatus.h:66
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
boost::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:88
Definition of an abstract state.
Definition: State.h:50
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:210
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
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:112
const Motion * parent
The parent motion in the tree.
Definition: Syclop.h:269
double weight
The probabilistic weight of this region, used when sampling from PDF.
Definition: Syclop.h:304
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68