PathSimplifier.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University, Inc.
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, Ryan Luna */
36 
37 #include "ompl/geometric/PathSimplifier.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include <algorithm>
40 #include <limits>
41 #include <cstdlib>
42 #include <cmath>
43 #include <map>
44 #include <utility>
45 
47  : si_(std::move(si)), freeStates_(true)
48 {
49  if (goal)
50  {
51  gsr_ = std::dynamic_pointer_cast<base::GoalSampleableRegion>(goal);
52  if (!gsr_)
53  OMPL_WARN("%s: Goal could not be cast to GoalSampleableRegion. Goal simplification will not be performed.",
54  __FUNCTION__);
55  }
56 }
57 
59 {
60  return freeStates_;
61 }
62 
64 {
65  freeStates_ = flag;
66 }
67 
68 /* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
69 void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
70 {
71  if (path.getStateCount() < 3)
72  return;
73 
74  const base::SpaceInformationPtr &si = path.getSpaceInformation();
75  std::vector<base::State *> &states = path.getStates();
76 
77  base::State *temp1 = si->allocState();
78  base::State *temp2 = si->allocState();
79 
80  for (unsigned int s = 0; s < maxSteps; ++s)
81  {
82  path.subdivide();
83 
84  unsigned int i = 2, u = 0, n1 = states.size() - 1;
85  while (i < n1)
86  {
87  if (si->isValid(states[i - 1]))
88  {
89  si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
90  si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
91  si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
92  if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
93  {
94  if (si->distance(states[i], temp1) > minChange)
95  {
96  si->copyState(states[i], temp1);
97  ++u;
98  }
99  }
100  }
101 
102  i += 2;
103  }
104 
105  if (u == 0)
106  break;
107  }
108 
109  si->freeState(temp1);
110  si->freeState(temp2);
111 }
112 
114  unsigned int maxEmptySteps, double rangeRatio)
115 {
116  if (path.getStateCount() < 3)
117  return false;
118 
119  if (maxSteps == 0)
120  maxSteps = path.getStateCount();
121 
122  if (maxEmptySteps == 0)
123  maxEmptySteps = path.getStateCount();
124 
125  bool result = false;
126  unsigned int nochange = 0;
127  const base::SpaceInformationPtr &si = path.getSpaceInformation();
128  std::vector<base::State *> &states = path.getStates();
129 
130  if (si->checkMotion(states.front(), states.back()))
131  {
132  if (freeStates_)
133  for (std::size_t i = 2; i < states.size(); ++i)
134  si->freeState(states[i - 1]);
135  std::vector<base::State *> newStates(2);
136  newStates[0] = states.front();
137  newStates[1] = states.back();
138  states.swap(newStates);
139  result = true;
140  }
141  else
142  for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
143  {
144  int count = states.size();
145  int maxN = count - 1;
146  int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
147 
148  int p1 = rng_.uniformInt(0, maxN);
149  int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
150  if (abs(p1 - p2) < 2)
151  {
152  if (p1 < maxN - 1)
153  p2 = p1 + 2;
154  else if (p1 > 1)
155  p2 = p1 - 2;
156  else
157  continue;
158  }
159 
160  if (p1 > p2)
161  std::swap(p1, p2);
162 
163  if (si->checkMotion(states[p1], states[p2]))
164  {
165  if (freeStates_)
166  for (int j = p1 + 1; j < p2; ++j)
167  si->freeState(states[j]);
168  states.erase(states.begin() + p1 + 1, states.begin() + p2);
169  nochange = 0;
170  result = true;
171  }
172  }
173  return result;
174 }
175 
177  unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
178 {
179  if (path.getStateCount() < 3)
180  return false;
181 
182  if (maxSteps == 0)
183  maxSteps = path.getStateCount();
184 
185  if (maxEmptySteps == 0)
186  maxEmptySteps = path.getStateCount();
187 
188  const base::SpaceInformationPtr &si = path.getSpaceInformation();
189  std::vector<base::State *> &states = path.getStates();
190 
191  // dists[i] contains the cumulative length of the path up to and including state i
192  std::vector<double> dists(states.size(), 0.0);
193  for (unsigned int i = 1; i < dists.size(); ++i)
194  dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);
195  // Sampled states closer than 'threshold' distance to any existing state in the path
196  // are snapped to the close state
197  double threshold = dists.back() * snapToVertex;
198  // The range (distance) of a single connection that will be attempted
199  double rd = rangeRatio * dists.back();
200 
201  base::State *temp0 = si->allocState();
202  base::State *temp1 = si->allocState();
203  bool result = false;
204  unsigned int nochange = 0;
205  // Attempt shortcutting maxSteps times or when no improvement is found after
206  // maxEmptySteps attempts, whichever comes first
207  for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
208  {
209  // Sample a random point anywhere along the path
210  base::State *s0 = nullptr;
211  int index0 = -1;
212  double t0 = 0.0;
213  double p0 = rng_.uniformReal(0.0, dists.back()); // sample a random point (p0) along the path
214  auto pit = std::lower_bound(dists.begin(), dists.end(), p0); // find the NEXT waypoint after the random point
215  int pos0 = pit == dists.end() ? dists.size() - 1 :
216  pit - dists.begin(); // get the index of the NEXT waypoint after the point
217 
218  if (pos0 == 0 || dists[pos0] - p0 < threshold) // snap to the NEXT waypoint
219  index0 = pos0;
220  else
221  {
222  while (pos0 > 0 && p0 < dists[pos0])
223  --pos0;
224  if (p0 - dists[pos0] < threshold) // snap to the PREVIOUS waypoint
225  index0 = pos0;
226  }
227 
228  // Sample a random point within rd distance of the previously sampled point
229  base::State *s1 = nullptr;
230  int index1 = -1;
231  double t1 = 0.0;
232  double p1 = rng_.uniformReal(std::max(0.0, p0 - rd),
233  std::min(p0 + rd, dists.back())); // sample a random point (p1) near p0
234  pit = std::lower_bound(dists.begin(), dists.end(), p1); // find the NEXT waypoint after the random point
235  int pos1 = pit == dists.end() ? dists.size() - 1 :
236  pit - dists.begin(); // get the index of the NEXT waypoint after the point
237 
238  if (pos1 == 0 || dists[pos1] - p1 < threshold) // snap to the NEXT waypoint
239  index1 = pos1;
240  else
241  {
242  while (pos1 > 0 && p1 < dists[pos1])
243  --pos1;
244  if (p1 - dists[pos1] < threshold) // snap to the PREVIOUS waypoint
245  index1 = pos1;
246  }
247 
248  // Don't waste time on points that are on the same path segment
249  if (pos0 == pos1 || index0 == pos1 || index1 == pos0 || pos0 + 1 == index1 || pos1 + 1 == index0 ||
250  (index0 >= 0 && index1 >= 0 && abs(index0 - index1) < 2))
251  continue;
252 
253  // Get the state pointer for p0
254  if (index0 >= 0)
255  s0 = states[index0];
256  else
257  {
258  t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
259  si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
260  s0 = temp0;
261  }
262 
263  // Get the state pointer for p1
264  if (index1 >= 0)
265  s1 = states[index1];
266  else
267  {
268  t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
269  si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
270  s1 = temp1;
271  }
272 
273  // Check for validity between s0 and s1
274  if (si->checkMotion(s0, s1))
275  {
276  if (pos0 > pos1)
277  {
278  std::swap(pos0, pos1);
279  std::swap(index0, index1);
280  std::swap(s0, s1);
281  std::swap(t0, t1);
282  }
283 
284  // Modify the path with the new, shorter result
285  if (index0 < 0 && index1 < 0)
286  {
287  if (pos0 + 1 == pos1)
288  {
289  si->copyState(states[pos1], s0);
290  states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
291  }
292  else
293  {
294  if (freeStates_)
295  for (int j = pos0 + 2; j < pos1; ++j)
296  si->freeState(states[j]);
297  si->copyState(states[pos0 + 1], s0);
298  si->copyState(states[pos1], s1);
299  states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
300  }
301  }
302  else if (index0 >= 0 && index1 >= 0)
303  {
304  if (freeStates_)
305  for (int j = index0 + 1; j < index1; ++j)
306  si->freeState(states[j]);
307  states.erase(states.begin() + index0 + 1, states.begin() + index1);
308  }
309  else if (index0 < 0 && index1 >= 0)
310  {
311  if (freeStates_)
312  for (int j = pos0 + 2; j < index1; ++j)
313  si->freeState(states[j]);
314  si->copyState(states[pos0 + 1], s0);
315  states.erase(states.begin() + pos0 + 2, states.begin() + index1);
316  }
317  else if (index0 >= 0 && index1 < 0)
318  {
319  if (freeStates_)
320  for (int j = index0 + 1; j < pos1; ++j)
321  si->freeState(states[j]);
322  si->copyState(states[pos1], s1);
323  states.erase(states.begin() + index0 + 1, states.begin() + pos1);
324  }
325 
326  // fix the helper variables
327  dists.resize(states.size(), 0.0);
328  for (unsigned int j = pos0 + 1; j < dists.size(); ++j)
329  dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
330  threshold = dists.back() * snapToVertex;
331  rd = rangeRatio * dists.back();
332  result = true;
333  nochange = 0;
334  }
335  }
336 
337  si->freeState(temp1);
338  si->freeState(temp0);
339  return result;
340 }
341 
343  unsigned int maxEmptySteps)
344 {
345  if (path.getStateCount() < 3)
346  return false;
347 
348  if (maxSteps == 0)
349  maxSteps = path.getStateCount();
350 
351  if (maxEmptySteps == 0)
352  maxEmptySteps = path.getStateCount();
353 
354  const base::SpaceInformationPtr &si = path.getSpaceInformation();
355  std::vector<base::State *> &states = path.getStates();
356 
357  // compute pair-wise distances in path (construct only half the matrix)
358  std::map<std::pair<const base::State *, const base::State *>, double> distances;
359  for (unsigned int i = 0; i < states.size(); ++i)
360  for (unsigned int j = i + 2; j < states.size(); ++j)
361  distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
362 
363  bool result = false;
364  unsigned int nochange = 0;
365  for (unsigned int s = 0; s < maxSteps && nochange < maxEmptySteps; ++s, ++nochange)
366  {
367  // find closest pair of points
368  double minDist = std::numeric_limits<double>::infinity();
369  int p1 = -1;
370  int p2 = -1;
371  for (unsigned int i = 0; i < states.size(); ++i)
372  for (unsigned int j = i + 2; j < states.size(); ++j)
373  {
374  double d = distances[std::make_pair(states[i], states[j])];
375  if (d < minDist)
376  {
377  minDist = d;
378  p1 = i;
379  p2 = j;
380  }
381  }
382 
383  if (p1 >= 0 && p2 >= 0)
384  {
385  if (si->checkMotion(states[p1], states[p2]))
386  {
387  if (freeStates_)
388  for (int i = p1 + 1; i < p2; ++i)
389  si->freeState(states[i]);
390  states.erase(states.begin() + p1 + 1, states.begin() + p2);
391  result = true;
392  nochange = 0;
393  }
394  else
395  distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
396  }
397  else
398  break;
399  }
400  return result;
401 }
402 
404 {
406  simplify(path, neverTerminate);
407 }
408 
410 {
411  simplify(path, base::timedPlannerTerminationCondition(maxTime));
412 }
413 
415 {
416  if (path.getStateCount() < 3)
417  return;
418 
419  bool tryMore = true;
420  while ((ptc == false) && tryMore)
421  {
422  // if the space is metric, we can do some additional smoothing
423  if ((ptc == false) && si_->getStateSpace()->isMetricSpace())
424  {
425  bool metricTryMore = true;
426  unsigned int times = 0;
427  do
428  {
429  bool shortcut = shortcutPath(path); // split path segments, not just vertices
430  bool better_goal =
431  gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
432 
433  metricTryMore = shortcut || better_goal;
434  } while ((ptc == false) && ++times <= 5 && metricTryMore);
435 
436  // smooth the path with BSpline interpolation
437  if (ptc == false)
438  smoothBSpline(path, 3, path.length() / 100.0);
439 
440  if (ptc == false)
441  {
442  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
443  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
444  if (!p.second)
445  {
446  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
447  }
448  else if (!p.first)
449  OMPL_DEBUG(
450  "The solution path was slightly touching on an invalid region of the state space, but it was "
451  "successfully fixed.");
452  }
453  }
454 
455  // try a randomized step of connecting vertices
456  if (ptc == false)
457  tryMore = reduceVertices(path);
458 
459  // try to collapse close-by vertices
460  if (ptc == false)
461  collapseCloseVertices(path);
462 
463  // try to reduce verices some more, if there is any point in doing so
464  unsigned int times = 0;
465  while ((ptc == false) && tryMore && ++times <= 5)
466  tryMore = reduceVertices(path);
467 
468  if ((ptc == false) && si_->getStateSpace()->isMetricSpace())
469  {
470  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
471  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
472  if (!p.second)
473  {
474  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
475  }
476  else if (!p.first)
477  OMPL_DEBUG(
478  "The solution path was slightly touching on an invalid region of the state space, but it was "
479  "successfully fixed.");
480  }
481 
482  }
483 }
484 
485 bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts,
486  double rangeRatio, double snapToVertex)
487 {
488  return findBetterGoal(path, base::timedPlannerTerminationCondition(maxTime), samplingAttempts, rangeRatio,
489  snapToVertex);
490 }
491 
493  unsigned int samplingAttempts, double rangeRatio,
494  double snapToVertex)
495 {
496  if (path.getStateCount() < 2)
497  return false;
498 
499  if (!gsr_)
500  {
501  OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
502  return false;
503  }
504 
505  unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
506  unsigned int failedTries = 0;
507  bool betterGoal = false;
508 
509  const base::StateSpacePtr &ss = si_->getStateSpace();
510  std::vector<base::State *> &states = path.getStates();
511 
512  // dists[i] contains the cumulative length of the path up to and including state i
513  std::vector<double> dists(states.size(), 0.0);
514  for (unsigned int i = 1; i < dists.size(); ++i)
515  dists[i] = dists[i - 1] + si_->distance(states[i - 1], states[i]);
516 
517  // Sampled states closer than 'threshold' distance to any existing state in the path
518  // are snapped to the close state
519  double threshold = dists.back() * snapToVertex;
520  // The range (distance) of a single connection that will be attempted
521  double rd = rangeRatio * dists.back();
522 
523  base::State *temp = si_->allocState();
524  base::State *tempGoal = si_->allocState();
525 
526  while (!ptc && failedTries++ < maxGoals && !betterGoal)
527  {
528  gsr_->sampleGoal(tempGoal);
529 
530  // Goal state is not compatible with the start state
531  if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
532  continue;
533 
534  unsigned int numSamples = 0;
535  while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
536  {
537  // sample a state within rangeRatio
538  double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0),
539  dists.back()); // Sample a random point within rd of the end of the path
540 
541  auto end = std::lower_bound(dists.begin(), dists.end(), t);
542  auto start = end;
543  while (start != dists.begin() && *start >= t)
544  start -= 1;
545 
546  unsigned int startIndex = start - dists.begin();
547  unsigned int endIndex = end - dists.begin();
548 
549  // Snap the random point to the nearest vertex, if within the threshold
550  if (t - (*start) < threshold) // snap to the starting waypoint
551  endIndex = startIndex;
552  if ((*end) - t < threshold) // snap to the ending waypoint
553  startIndex = endIndex;
554 
555  // Compute the state value and the accumulated cost up to that state
556  double costToCome = dists[startIndex];
557  base::State *state;
558  if (startIndex == endIndex)
559  {
560  state = states[startIndex];
561  }
562  else
563  {
564  double tSeg = (t - (*start)) / (*end - *start);
565  ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
566  state = temp;
567 
568  costToCome += si_->distance(states[startIndex], state);
569  }
570 
571  double costToGo = si_->distance(state, tempGoal);
572  double candidateCost = costToCome + costToGo;
573 
574  // Make sure we improve before attempting validation
575  if (dists.back() - candidateCost > std::numeric_limits<float>::epsilon() &&
576  si_->checkMotion(state, tempGoal))
577  {
578  // insert the new states
579  if (startIndex == endIndex)
580  {
581  // new intermediate state
582  si_->copyState(states[startIndex], state);
583  // new goal state
584  si_->copyState(states[startIndex + 1], tempGoal);
585 
586  if (freeStates_)
587  {
588  for (size_t i = startIndex + 2; i < states.size(); ++i)
589  si_->freeState(states[i]);
590  }
591  states.erase(states.begin() + startIndex + 2, states.end());
592  }
593  else
594  {
595  // overwriting the end of the segment with the new state
596  si_->copyState(states[endIndex], state);
597  if (endIndex == states.size() - 1)
598  {
599  path.append(tempGoal);
600  }
601  else
602  {
603  // adding goal new goal state
604  si_->copyState(states[endIndex + 1], tempGoal);
605  if (freeStates_)
606  {
607  for (size_t i = endIndex + 2; i < states.size(); ++i)
608  si_->freeState(states[i]);
609  }
610  states.erase(states.begin() + endIndex + 2, states.end());
611  }
612  }
613 
614  // fix the helper variables
615  dists.resize(states.size(), 0.0);
616  for (unsigned int j = std::max(1u, startIndex); j < dists.size(); ++j)
617  dists[j] = dists[j - 1] + si_->distance(states[j - 1], states[j]);
618 
619  betterGoal = true;
620  }
621  }
622  }
623 
624  si_->freeState(temp);
625  si_->freeState(tempGoal);
626 
627  return betterGoal;
628 }
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification...
bool findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts=10, double rangeRatio=0.33, double snapToVertex=0.005)
Attempt to improve the solution path by sampling a new goal state and connecting this state to the so...
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met...
bool shortcutPath(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33, double snapToVertex=0.005)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
void simplify(PathGeometric &path, double maxTime)
Run simplification algorithms on the path for at most maxTime seconds.
A shared pointer wrapper for ompl::base::StateSpace.
void simplifyMax(PathGeometric &path)
Given a path, attempt to remove vertices from it while keeping the path valid. Then, try to smooth the path. This function applies the same set of default operations to the path, except in non-metric spaces, with the intention of simplifying it. In non-metric spaces, some operations are skipped because they do not work correctly when the triangle inequality may not hold.
PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal=ompl::base::GoalPtr())
Create an instance for a specified space information. Optionally, a GoalSampleableRegion may be passe...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
Abstract definition of a goal region that can be sampled.
bool reduceVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
A shared pointer wrapper for ompl::base::Goal.
Definition of a geometric path.
Definition: PathGeometric.h:60
bool collapseCloseVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
void smoothBSpline(PathGeometric &path, unsigned int maxSteps=5, double minChange=std::numeric_limits< double >::epsilon())
Given a path, attempt to smooth it (the validity of the path is maintained).
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...