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/PDF.h"
41 #include "ompl/tools/config/SelfConfig.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, std::string("8:1000"));
94 }
95 
96 ompl::geometric::PRM::~PRM(void)
97 {
98  freeMemory();
99 }
100 
102 {
103  Planner::setup();
104  if (!nn_)
105  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
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 sol_cost = -1.;
302  bool sol_cost_set = false;
303  foreach (Vertex start, starts)
304  {
305  foreach (Vertex goal, goals)
306  {
307  if (boost::same_component(start, goal, disjointSets_) &&
308  g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
309  {
310  // If there is an optimization objective, check it
311  if (pdef_->hasOptimizationObjective())
312  {
313  base::PathPtr p = constructSolution(start, goal);
314  double obj_cost = pdef_->getOptimizationObjective()->getCost(p);
315  if (pdef_->getOptimizationObjective()->isSatisfied(obj_cost)) // Sufficient solution
316  {
317  solution = p;
318  return true;
319  }
320  else
321  {
322  if (solution && !sol_cost_set)
323  {
324  sol_cost = pdef_->getOptimizationObjective()->getCost(solution);
325  sol_cost_set = true;
326  }
327 
328  if (!solution || obj_cost < sol_cost)
329  {
330  solution = p;
331  sol_cost = obj_cost;
332  sol_cost_set = true;
333  }
334  }
335  }
336  else // Accept the solution, regardless of cost
337  {
338  solution = constructSolution(start, goal);
339  return true;
340  }
341  }
342  }
343  }
344 
345  return false;
346 }
347 
349 {
350  return addedSolution_;
351 }
352 
354 {
355  checkValidity();
356  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
357 
358  if (!goal)
359  {
360  OMPL_ERROR("Goal undefined or unknown type of goal");
362  }
363 
364  // Add the valid start states as milestones
365  while (const base::State *st = pis_.nextStart())
366  startM_.push_back(addMilestone(si_->cloneState(st)));
367 
368  if (startM_.size() == 0)
369  {
370  OMPL_ERROR("There are no valid initial states!");
372  }
373 
374  if (!goal->couldSample())
375  {
376  OMPL_ERROR("Insufficient states in sampleable goal region");
378  }
379 
380  // Ensure there is at least one valid goal state
381  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
382  {
383  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
384  if (st)
385  goalM_.push_back(addMilestone(si_->cloneState(st)));
386 
387  if (goalM_.empty())
388  {
389  OMPL_ERROR("Unable to find any valid goal states");
391  }
392  }
393 
394  if (!sampler_)
395  sampler_ = si_->allocValidStateSampler();
396  if (!simpleSampler_)
397  simpleSampler_ = si_->allocStateSampler();
398 
399  unsigned int nrStartStates = boost::num_vertices(g_);
400  OMPL_INFORM("Starting with %u states", nrStartStates);
401 
402  std::vector<base::State*> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
403  si_->allocStates(xstates);
404  bool grow = true;
405 
406  // Reset addedSolution_ member and create solution checking thread
407  addedSolution_ = false;
408  base::PathPtr sol;
409  sol.reset();
410  boost::thread slnThread (boost::bind(&PRM::checkForSolution, this, ptc, boost::ref(sol)));
411 
412  // construct new planner termination condition that fires when the given ptc is true, or a solution is found
414 
415  while (ptcOrSolutionFound() == false)
416  {
417  // maintain a 2:1 ratio for growing/expansion of roadmap
418  // call growRoadmap() twice as long for every call of expandRoadmap()
419  if (grow)
421  else
423  grow = !grow;
424  }
425 
426  // Ensure slnThread is ceased before exiting solve
427  slnThread.join();
428 
429  OMPL_INFORM("Created %u states", boost::num_vertices(g_) - nrStartStates);
430 
431  if (sol)
432  {
433  if (addedNewSolution())
434  pdef_->addSolutionPath (sol);
435  else
436  // the solution is exact, but not as short as we'd like it to be
437  pdef_->addSolutionPath (sol, true, 0.0);
438  }
439 
440  si_->freeStates(xstates);
441 
442  // Return true if any solution was found.
444 }
445 
446 ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state)
447 {
448  graphMutex_.lock();
449  Vertex m = boost::add_vertex(g_);
450  stateProperty_[m] = state;
451  totalConnectionAttemptsProperty_[m] = 1;
452  successfulConnectionAttemptsProperty_[m] = 0;
453 
454  // Initialize to its own (dis)connected component.
455  disjointSets_.make_set(m);
456  graphMutex_.unlock();
457 
458  // Which milestones will we attempt to connect to?
459  if (!connectionStrategy_)
460  throw Exception(name_, "No connection strategy!");
461 
462  const std::vector<Vertex>& neighbors = connectionStrategy_(m);
463 
464  foreach (Vertex n, neighbors)
465  if ((boost::same_component(m, n, disjointSets_) || connectionFilter_(m, n)))
466  {
467  totalConnectionAttemptsProperty_[m]++;
468  totalConnectionAttemptsProperty_[n]++;
469  if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
470  {
471  successfulConnectionAttemptsProperty_[m]++;
472  successfulConnectionAttemptsProperty_[n]++;
473  const double weight = distanceFunction(m, n);
474  const unsigned int id = maxEdgeID_++;
475  const Graph::edge_property_type properties(weight, id);
476 
477  graphMutex_.lock();
478  boost::add_edge(m, n, properties, g_);
479  uniteComponents(n, m);
480  graphMutex_.unlock();
481  }
482  }
483 
484  nn_->add(m);
485  return m;
486 }
487 
488 void ompl::geometric::PRM::uniteComponents(Vertex m1, Vertex m2)
489 {
490  disjointSets_.union_set(m1, m2);
491 }
492 
493 ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex start, const Vertex goal) const
494 {
495  PathGeometric *p = new PathGeometric(si_);
496 
497  graphMutex_.lock();
498  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
499 
500  boost::astar_search(g_, start,
501  boost::bind(&PRM::distanceFunction, this, _1, goal),
502  boost::predecessor_map(prev));
503  graphMutex_.unlock();
504 
505  if (prev[goal] == goal)
506  throw Exception(name_, "Could not find solution path");
507  else
508  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
509  p->append(stateProperty_[pos]);
510  p->append(stateProperty_[start]);
511  p->reverse();
512 
513  return base::PathPtr(p);
514 }
515 
517 {
518  Planner::getPlannerData(data);
519 
520  // Explicitly add start and goal states:
521  for (size_t i = 0; i < startM_.size(); ++i)
522  data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]]));
523 
524  for (size_t i = 0; i < goalM_.size(); ++i)
525  data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]]));
526 
527  // Adding edges and all other vertices simultaneously
528  foreach(const Edge e, boost::edges(g_))
529  {
530  const Vertex v1 = boost::source(e, g_);
531  const Vertex v2 = boost::target(e, g_);
532  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]),
533  base::PlannerDataVertex(stateProperty_[v2]));
534 
535  // Add the reverse edge, since we're constructing an undirected roadmap
536  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]),
537  base::PlannerDataVertex(stateProperty_[v1]));
538  }
539 }