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 
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;
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 
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 
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 {
412 }
413 
415 {
416  if (path.getStateCount() < 3)
417  return;
418 
419  // try a randomized step of connecting vertices
420  bool tryMore = false;
421  if (ptc == false)
422  tryMore = reduceVertices(path);
423 
424  // try to collapse close-by vertices
425  if (ptc == false)
426  collapseCloseVertices(path);
427 
428  // try to reduce verices some more, if there is any point in doing so
429  int times = 0;
430  while (tryMore && ptc == false && ++times <= 5)
431  tryMore = reduceVertices(path);
432 
433  // if the space is metric, we can do some additional smoothing
434  if (si_->getStateSpace()->isMetricSpace())
435  {
436  bool tryMore = true;
437  unsigned int times = 0;
438  do
439  {
440  bool shortcut = shortcutPath(path); // split path segments, not just vertices
441  bool better_goal = gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
442 
443  tryMore = shortcut || better_goal;
444  } while (ptc == false && tryMore && ++times <= 5);
445 
446  // smooth the path with BSpline interpolation
447  if (ptc == false)
448  smoothBSpline(path, 3, path.length() / 100.0);
449 
450  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
451  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
452  if (!p.second)
453  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
454  else if (!p.first)
455  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was "
456  "successfully fixed.");
457  }
458 }
459 
460 bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts,
461  double rangeRatio, double snapToVertex)
462 {
463  return findBetterGoal(path, base::timedPlannerTerminationCondition(maxTime), samplingAttempts, rangeRatio,
464  snapToVertex);
465 }
466 
468  unsigned int samplingAttempts, double rangeRatio,
469  double snapToVertex)
470 {
471  if (path.getStateCount() < 2)
472  return false;
473 
474  if (!gsr_)
475  {
476  OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
477  return false;
478  }
479 
480  unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
481  unsigned int failedTries = 0;
482  bool betterGoal = false;
483 
484  const base::StateSpacePtr &ss = si_->getStateSpace();
485  std::vector<base::State *> &states = path.getStates();
486 
487  // dists[i] contains the cumulative length of the path up to and including state i
488  std::vector<double> dists(states.size(), 0.0);
489  for (unsigned int i = 1; i < dists.size(); ++i)
490  dists[i] = dists[i - 1] + si_->distance(states[i - 1], states[i]);
491 
492  // Sampled states closer than 'threshold' distance to any existing state in the path
493  // are snapped to the close state
494  double threshold = dists.back() * snapToVertex;
495  // The range (distance) of a single connection that will be attempted
496  double rd = rangeRatio * dists.back();
497 
498  base::State *temp = si_->allocState();
499  base::State *tempGoal = si_->allocState();
500 
501  while (!ptc && failedTries++ < maxGoals && !betterGoal)
502  {
503  gsr_->sampleGoal(tempGoal);
504 
505  // Goal state is not compatible with the start state
506  if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
507  continue;
508 
509  unsigned int numSamples = 0;
510  while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
511  {
512  // sample a state within rangeRatio
513  double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0),
514  dists.back()); // Sample a random point within rd of the end of the path
515 
516  auto end = std::lower_bound(dists.begin(), dists.end(), t);
517  auto start = end;
518  while (start != dists.begin() && *start >= t)
519  start -= 1;
520 
521  unsigned int startIndex = start - dists.begin();
522  unsigned int endIndex = end - dists.begin();
523 
524  // Snap the random point to the nearest vertex, if within the threshold
525  if (t - (*start) < threshold) // snap to the starting waypoint
526  endIndex = startIndex;
527  if ((*end) - t < threshold) // snap to the ending waypoint
528  startIndex = endIndex;
529 
530  // Compute the state value and the accumulated cost up to that state
531  double costToCome = dists[startIndex];
532  base::State *state;
533  if (startIndex == endIndex)
534  {
535  state = states[startIndex];
536  }
537  else
538  {
539  double tSeg = (t - (*start)) / (*end - *start);
540  ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
541  state = temp;
542 
543  costToCome += si_->distance(states[startIndex], state);
544  }
545 
546  double costToGo = si_->distance(state, tempGoal);
547  double candidateCost = costToCome + costToGo;
548 
549  // Make sure we improve before attempting validation
550  if (dists.back() - candidateCost > std::numeric_limits<float>::epsilon() &&
551  si_->checkMotion(state, tempGoal))
552  {
553  // insert the new states
554  if (startIndex == endIndex)
555  {
556  // new intermediate state
557  si_->copyState(states[startIndex], state);
558  // new goal state
559  si_->copyState(states[startIndex + 1], tempGoal);
560 
561  if (freeStates_)
562  {
563  for (size_t i = startIndex + 2; i < states.size(); ++i)
564  si_->freeState(states[i]);
565  }
566  states.erase(states.begin() + startIndex + 2, states.end());
567  }
568  else
569  {
570  // overwriting the end of the segment with the new state
571  si_->copyState(states[endIndex], state);
572  if (endIndex == states.size() - 1)
573  {
574  path.append(tempGoal);
575  }
576  else
577  {
578  // adding goal new goal state
579  si_->copyState(states[endIndex + 1], tempGoal);
580  if (freeStates_)
581  {
582  for (size_t i = endIndex + 2; i < states.size(); ++i)
583  si_->freeState(states[i]);
584  }
585  states.erase(states.begin() + endIndex + 2, states.end());
586  }
587  }
588 
589  // fix the helper variables
590  dists.resize(states.size(), 0.0);
591  for (unsigned int j = std::max(1u, startIndex); j < dists.size(); ++j)
592  dists[j] = dists[j - 1] + si_->distance(states[j - 1], states[j]);
593 
594  betterGoal = true;
595  }
596  }
597  }
598 
599  si_->freeState(temp);
600  si_->freeState(tempGoal);
601 
602  return betterGoal;
603 }
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...
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
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...
RNG rng_
Instance of random number generator.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
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.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
base::State * getState(unsigned int index)
Get the state located at index along the path.
const SpaceInformationPtr & getSpaceInformation() const
Get the space information associated to this class.
Definition: Path.h:83
std::pair< bool, bool > checkAndRepair(unsigned int attempts)
Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invali...
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...
base::SpaceInformationPtr si_
The space information this path simplifier uses.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:74
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
void subdivide()
Add a state at the middle of each segment.
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...
bool freeStates_
Flag indicating whether the states removed from a motion should be freed.
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:81
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...
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...