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  // 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.
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,...
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...
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)
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...