All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PRM.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: Ioan Sucan, James D. Marble */
36 
37 #include "ompl/geometric/planners/prm/PRM.h"
38 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/datastructures/NearestNeighborsGNAT.h"
41 #include "ompl/datastructures/PDF.h"
42 #include <boost/lambda/bind.hpp>
43 #include <boost/graph/astar_search.hpp>
44 #include <boost/graph/incremental_components.hpp>
45 #include <boost/property_map/vector_property_map.hpp>
46 #include <boost/foreach.hpp>
47 #include <boost/thread.hpp>
48 
49 #define foreach BOOST_FOREACH
50 #define foreach_reverse BOOST_REVERSE_FOREACH
51 
52 namespace ompl
53 {
54  namespace magic
55  {
56 
60  static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK = 2;
61 
64  static const unsigned int MAX_RANDOM_BOUNCE_STEPS = 5;
65 
68  static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
69 
71  static const double ROADMAP_BUILD_TIME = 0.2;
72  }
73 }
74 
76  base::Planner(si, "PRM"),
77  starStrategy_(starStrategy),
78  stateProperty_(boost::get(vertex_state_t(), g_)),
79  totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_)),
80  successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_)),
81  weightProperty_(boost::get(boost::edge_weight, g_)),
82  edgeIDProperty_(boost::get(boost::edge_index, g_)),
83  disjointSets_(boost::get(boost::vertex_rank, g_),
84  boost::get(boost::vertex_predecessor, g_)),
85  maxEdgeID_(0),
86  userSetConnectionStrategy_(false),
87  addedSolution_(false)
88 {
91  specs_.optimizingPaths = true;
92 
93  Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors);
94 }
95 
96 ompl::geometric::PRM::~PRM(void)
97 {
98  freeMemory();
99 }
100 
102 {
103  Planner::setup();
104  if (!nn_)
105  nn_.reset(new NearestNeighborsGNAT<Vertex>());
106  nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
107  if (!connectionStrategy_)
108  {
109  if (starStrategy_)
110  connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension());
111  else
112  connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
113  }
114  if (!connectionFilter_)
115  connectionFilter_ = boost::lambda::constant(true);
116 }
117 
119 {
120  if (!setup_)
121  setup();
122  connectionStrategy_ = KStrategy<Vertex>(k, nn_);
123 }
124 
126 {
127  Planner::setProblemDefinition(pdef);
128  clearQuery();
129 }
130 
132 {
133  startM_.clear();
134  goalM_.clear();
135  pis_.restart();
136 }
137 
139 {
140  Planner::clear();
141  sampler_.reset();
142  simpleSampler_.reset();
143  freeMemory();
144  if (nn_)
145  nn_->clear();
146  clearQuery();
147  maxEdgeID_ = 0;
148 }
149 
151 {
152  foreach (Vertex v, boost::vertices(g_))
153  si_->freeState(stateProperty_[v]);
154  g_.clear();
155 }
156 
157 void ompl::geometric::PRM::expandRoadmap(double expandTime)
158 {
159  expandRoadmap(base::timedPlannerTerminationCondition(expandTime));
160 }
161 
163 {
164  if (!simpleSampler_)
165  simpleSampler_ = si_->allocStateSampler();
166 
167  std::vector<base::State*> states(magic::MAX_RANDOM_BOUNCE_STEPS);
168  si_->allocStates(states);
169  expandRoadmap(ptc, states);
170  si_->freeStates(states);
171 }
172 
174  std::vector<base::State*> &workStates)
175 {
176  // construct a probability distribution over the vertices in the roadmap
177  // as indicated in
178  // "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
179  // Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
180 
181  PDF<Vertex> pdf;
182  foreach (Vertex v, boost::vertices(g_))
183  {
184  const unsigned int t = totalConnectionAttemptsProperty_[v];
185  pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
186  }
187 
188  if (pdf.empty())
189  return;
190 
191  while (ptc() == false)
192  {
193  Vertex v = pdf.sample(rng_.uniform01());
194  unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
195  if (s > 0)
196  {
197  s--;
198  Vertex last = addMilestone(si_->cloneState(workStates[s]));
199 
200  graphMutex_.lock();
201  for (unsigned int i = 0 ; i < s ; ++i)
202  {
203  // add the vertex along the bouncing motion
204  Vertex m = boost::add_vertex(g_);
205  stateProperty_[m] = si_->cloneState(workStates[i]);
206  totalConnectionAttemptsProperty_[m] = 1;
207  successfulConnectionAttemptsProperty_[m] = 0;
208  disjointSets_.make_set(m);
209 
210  // add the edge to the parent vertex
211  const double weight = distanceFunction(v, m);
212  const unsigned int id = maxEdgeID_++;
213  const Graph::edge_property_type properties(weight, id);
214  boost::add_edge(v, m, properties, g_);
215  uniteComponents(v, m);
216 
217  // add the vertex to the nearest neighbors data structure
218  nn_->add(m);
219  v = m;
220  }
221 
222  // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
223  // we add an edge
224  if (s > 0 || !boost::same_component(v, last, disjointSets_))
225  {
226  // add the edge to the parent vertex
227  const double weight = distanceFunction(v, last);
228  const unsigned int id = maxEdgeID_++;
229  const Graph::edge_property_type properties(weight, id);
230  boost::add_edge(v, last, properties, g_);
231  uniteComponents(v, last);
232  }
233  graphMutex_.unlock();
234  }
235  }
236 }
237 
239 {
240  growRoadmap(base::timedPlannerTerminationCondition(growTime));
241 }
242 
244 {
245  if (!isSetup())
246  setup();
247  if (!sampler_)
248  sampler_ = si_->allocValidStateSampler();
249 
250  base::State *workState = si_->allocState();
251  growRoadmap (ptc, workState);
252  si_->freeState(workState);
253 }
254 
256  base::State *workState)
257 {
258  while (ptc() == false)
259  {
260  // search for a valid state
261  bool found = false;
262  while (!found && ptc() == false)
263  {
264  unsigned int attempts = 0;
265  do
266  {
267  found = sampler_->sample(workState);
268  attempts++;
269  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
270  }
271  // add it as a milestone
272  if (found)
273  addMilestone(si_->cloneState(workState));
274  }
275 }
276 
278  base::PathPtr &solution)
279 {
280  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
281  while (!ptc() && !addedSolution_)
282  {
283  // Check for any new goal states
284  if (goal->maxSampleCount() > goalM_.size())
285  {
286  const base::State *st = pis_.nextGoal();
287  if (st)
288  goalM_.push_back(addMilestone(si_->cloneState(st)));
289  }
290 
291  // Check for a solution
292  addedSolution_ = haveSolution (startM_, goalM_, solution);
293  // Sleep for 1ms
294  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
295  }
296 }
297 
298 bool ompl::geometric::PRM::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution)
299 {
300  base::Goal *g = pdef_->getGoal().get();
301  double sl = -1.0; // cache for solution length
302  foreach (Vertex start, starts)
303  {
304  foreach (Vertex goal, goals)
305  {
306  if (boost::same_component(start, goal, disjointSets_) &&
307  g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
308  {
309  // If there is a maximum acceptable path length, check the solution length
310  if (g->getMaximumPathLength() < std::numeric_limits<double>::infinity())
311  {
312  base::PathPtr p = constructSolution(start, goal);
313  double pl = p->length(); // avoid computing path length multiple times
314  if (pl < g->getMaximumPathLength()) // Sufficient solution
315  {
316  solution = p;
317  return true;
318  }
319  else
320  {
321  if (solution && sl < 0.0)
322  sl = solution->length();
323  if (!solution || (solution && pl < sl)) // approximation
324  {
325  solution = p;
326  sl = pl;
327  }
328  }
329  }
330  else // Accept the solution, regardless of length
331  {
332  solution = constructSolution(start, goal);
333  return true;
334  }
335  }
336  }
337  }
338 
339  return false;
340 }
341 
343 {
344  return addedSolution_;
345 }
346 
348 {
349  checkValidity();
350  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
351 
352  if (!goal)
353  {
354  logError("Goal undefined or unknown type of goal");
356  }
357 
358  // Add the valid start states as milestones
359  while (const base::State *st = pis_.nextStart())
360  startM_.push_back(addMilestone(si_->cloneState(st)));
361 
362  if (startM_.size() == 0)
363  {
364  logError("There are no valid initial states!");
366  }
367 
368  if (!goal->couldSample())
369  {
370  logError("Insufficient states in sampleable goal region");
372  }
373 
374  // Ensure there is at least one valid goal state
375  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
376  {
377  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
378  if (st)
379  goalM_.push_back(addMilestone(si_->cloneState(st)));
380 
381  if (goalM_.empty())
382  {
383  logError("Unable to find any valid goal states");
385  }
386  }
387 
388  if (!sampler_)
389  sampler_ = si_->allocValidStateSampler();
390  if (!simpleSampler_)
391  simpleSampler_ = si_->allocStateSampler();
392 
393  unsigned int nrStartStates = boost::num_vertices(g_);
394  logInform("Starting with %u states", nrStartStates);
395 
396  std::vector<base::State*> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
397  si_->allocStates(xstates);
398  bool grow = true;
399 
400  // Reset addedSolution_ member and create solution checking thread
401  addedSolution_ = false;
402  base::PathPtr sol;
403  sol.reset();
404  boost::thread slnThread (boost::bind(&PRM::checkForSolution, this, ptc, boost::ref(sol)));
405 
406  // construct new planner termination condition that fires when the given ptc is true, or a solution is found
408 
409  while (ptcOrSolutionFound() == false)
410  {
411  // maintain a 2:1 ratio for growing/expansion of roadmap
412  // call growRoadmap() twice as long for every call of expandRoadmap()
413  if (grow)
415  else
417  grow = !grow;
418  }
419 
420  // Ensure slnThread is ceased before exiting solve
421  slnThread.join();
422 
423  logInform("Created %u states", boost::num_vertices(g_) - nrStartStates);
424 
425  if (sol)
426  {
427  if (addedNewSolution())
428  pdef_->addSolutionPath (sol);
429  else
430  // the solution is exact, but not as short as we'd like it to be
431  pdef_->addSolutionPath (sol, true, 0.0);
432  }
433 
434  si_->freeStates(xstates);
435 
436  // Return true if any solution was found.
438 }
439 
440 ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state)
441 {
442  graphMutex_.lock();
443  Vertex m = boost::add_vertex(g_);
444  stateProperty_[m] = state;
445  totalConnectionAttemptsProperty_[m] = 1;
446  successfulConnectionAttemptsProperty_[m] = 0;
447 
448  // Initialize to its own (dis)connected component.
449  disjointSets_.make_set(m);
450  graphMutex_.unlock();
451 
452  // Which milestones will we attempt to connect to?
453  if (!connectionStrategy_)
454  throw Exception(name_, "No connection strategy!");
455 
456  const std::vector<Vertex>& neighbors = connectionStrategy_(m);
457 
458  foreach (Vertex n, neighbors)
459  if ((boost::same_component(m, n, disjointSets_) || connectionFilter_(m, n)))
460  {
461  totalConnectionAttemptsProperty_[m]++;
462  totalConnectionAttemptsProperty_[n]++;
463  if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
464  {
465  successfulConnectionAttemptsProperty_[m]++;
466  successfulConnectionAttemptsProperty_[n]++;
467  const double weight = distanceFunction(m, n);
468  const unsigned int id = maxEdgeID_++;
469  const Graph::edge_property_type properties(weight, id);
470 
471  graphMutex_.lock();
472  boost::add_edge(m, n, properties, g_);
473  uniteComponents(n, m);
474  graphMutex_.unlock();
475  }
476  }
477 
478  nn_->add(m);
479  return m;
480 }
481 
482 void ompl::geometric::PRM::uniteComponents(Vertex m1, Vertex m2)
483 {
484  disjointSets_.union_set(m1, m2);
485 }
486 
487 ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex start, const Vertex goal) const
488 {
489  PathGeometric *p = new PathGeometric(si_);
490 
491  graphMutex_.lock();
492  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
493 
494  boost::astar_search(g_, start,
495  boost::bind(&PRM::distanceFunction, this, _1, goal),
496  boost::predecessor_map(prev));
497  graphMutex_.unlock();
498 
499  if (prev[goal] == goal)
500  throw Exception(name_, "Could not find solution path");
501  else
502  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
503  p->append(stateProperty_[pos]);
504  p->append(stateProperty_[start]);
505  p->reverse();
506 
507  return base::PathPtr(p);
508 }
509 
511 {
512  Planner::getPlannerData(data);
513 
514  // Explicitly add start and goal states:
515  for (size_t i = 0; i < startM_.size(); ++i)
516  data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]]));
517 
518  for (size_t i = 0; i < goalM_.size(); ++i)
519  data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]]));
520 
521  // Adding edges and all other vertices simultaneously
522  foreach(const Edge e, boost::edges(g_))
523  {
524  const Vertex v1 = boost::source(e, g_);
525  const Vertex v2 = boost::target(e, g_);
526  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]),
527  base::PlannerDataVertex(stateProperty_[v2]));
528 
529  // Add the reverse edge, since we're constructing an undirected roadmap
530  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]),
531  base::PlannerDataVertex(stateProperty_[v1]));
532  }
533 }