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, Louis Petit */
36 
37 #include "ompl/geometric/PathSimplifier.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
40 #include "ompl/base/StateSampler.h"
41 #include <algorithm>
42 #include <limits>
43 #include <cstdlib>
44 #include <cmath>
45 #include <map>
46 #include <utility>
47 
48 ompl::geometric::PathSimplifier::PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal,
49  const base::OptimizationObjectivePtr &obj)
50  : si_(std::move(si)), freeStates_(true)
51 {
52  if (goal)
53  {
54  gsr_ = std::dynamic_pointer_cast<base::GoalSampleableRegion>(goal);
55  if (!gsr_)
56  OMPL_WARN("%s: Goal could not be cast to GoalSampleableRegion. Goal simplification will not be performed.",
57  __FUNCTION__);
58  }
59  if (obj)
60  {
61  obj_ = obj;
62  }
63  else
64  {
65  obj_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
66  }
67 }
68 
69 bool ompl::geometric::PathSimplifier::freeStates() const
70 {
71  return freeStates_;
72 }
73 
74 void ompl::geometric::PathSimplifier::freeStates(bool flag)
75 {
76  freeStates_ = flag;
77 }
78 
79 /* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
80 void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
81 {
82  if (path.getStateCount() < 3)
83  return;
84 
85  const base::SpaceInformationPtr &si = path.getSpaceInformation();
86  std::vector<base::State *> &states = path.getStates();
87 
88  base::State *temp1 = si->allocState();
89  base::State *temp2 = si->allocState();
90 
91  for (unsigned int s = 0; s < maxSteps; ++s)
92  {
93  path.subdivide();
94 
95  unsigned int i = 2, u = 0, n1 = states.size() - 1;
96  while (i < n1)
97  {
98  if (si->isValid(states[i - 1]))
99  {
100  si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
101  si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
102  si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
103  if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
104  {
105  if (si->distance(states[i], temp1) > minChange)
106  {
107  si->copyState(states[i], temp1);
108  ++u;
109  }
110  }
111  }
112 
113  i += 2;
114  }
115 
116  if (u == 0)
117  break;
118  }
119 
120  si->freeState(temp1);
121  si->freeState(temp2);
122 }
123 
124 bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps,
125  unsigned int maxEmptySteps, double rangeRatio)
126 {
127  if (path.getStateCount() < 3)
128  return false;
129 
130  if (maxSteps == 0)
131  maxSteps = path.getStateCount();
132 
133  if (maxEmptySteps == 0)
134  maxEmptySteps = path.getStateCount();
135 
136  bool result = false;
137  unsigned int nochange = 0;
138  const base::SpaceInformationPtr &si = path.getSpaceInformation();
139  std::vector<base::State *> &states = path.getStates();
140 
141  if (si->checkMotion(states.front(), states.back()))
142  {
143  if (freeStates_)
144  for (std::size_t i = 2; i < states.size(); ++i)
145  si->freeState(states[i - 1]);
146  std::vector<base::State *> newStates(2);
147  newStates[0] = states.front();
148  newStates[1] = states.back();
149  states.swap(newStates);
150  result = true;
151  }
152  else
153  for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
154  {
155  int count = states.size();
156  int maxN = count - 1;
157  int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
158 
159  int p1 = rng_.uniformInt(0, maxN);
160  int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
161  if (abs(p1 - p2) < 2)
162  {
163  if (p1 < maxN - 1)
164  p2 = p1 + 2;
165  else if (p1 > 1)
166  p2 = p1 - 2;
167  else
168  continue;
169  }
170 
171  if (p1 > p2)
172  std::swap(p1, p2);
173 
174  if (si->checkMotion(states[p1], states[p2]))
175  {
176  if (freeStates_)
177  for (int j = p1 + 1; j < p2; ++j)
178  si->freeState(states[j]);
179  states.erase(states.begin() + p1 + 1, states.begin() + p2);
180  nochange = 0;
181  result = true;
182  }
183  }
184  return result;
185 }
186 
187 bool ompl::geometric::PathSimplifier::ropeShortcutPath(PathGeometric &path, double delta, double equivalenceTolerance)
188 {
189  if (path.getStateCount() < 3)
190  return false;
191 
192  const base::SpaceInformationPtr &si = path.getSpaceInformation();
193  std::vector<base::State *> &states = path.getStates();
194 
195  // Loops through the path segments to add intermediate nodes seperated by a distance delta apart.
196  // This is done by linearly interpolating between the two nodes
197  for (std::size_t i = 0; i < states.size() - 1; ++i)
198  {
199  double dist = si->distance(states[i], states[i + 1]);
200  if (dist > delta)
201  {
202  std::size_t numIntermediateStates = static_cast<std::size_t>(floor(dist / delta));
203  double t = 1.0 / (numIntermediateStates + 1);
204  for (std::size_t j = 0; j < numIntermediateStates; ++j)
205  {
206  base::State *newState = si->allocState();
207  si->getStateSpace()->interpolate(states[i], states[i + 1 + j], t * (j + 1), newState);
208  states.insert(states.begin() + i + 1 + j, newState);
209  }
210  i = i + numIntermediateStates;
211  }
212  }
213 
214  // Store the cumulative costs
215  // costs[i] contains the cumulative cost of the path up to and including state i
216  std::vector<base::Cost> costs(states.size(), obj_->identityCost());
217  for (std::size_t i = 1; i < costs.size(); ++i)
218  {
219  costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
220  }
221 
222  bool result = false;
223  ompl::base::Cost equivalenceCost(equivalenceTolerance * delta);
224 
225  // Loops through the path nodes to perform shortcutting, considering the farthest nodes first.
226  for (std::size_t i = 0; i < states.size() - 2; ++i)
227  {
228  // std::cout << "i = " << i << "/" << states.size() - 1 << std::endl;
229  for (std::size_t j = states.size() - 1; j > i + 1; --j)
230  {
231  // Check if the shortcut is valid
232  if (si->checkMotion(states[i], states[j]))
233  {
234  base::Cost shortcutCost = obj_->motionCost(states[i], states[j]);
235  base::Cost alongPath = obj_->subtractCosts(costs[j], costs[i]);
236 
237  // Check if the path segment i-j is already optimal and break out of j loop if it is.
238  if (obj_->isCostBetterThan(obj_->subtractCosts(alongPath, shortcutCost), equivalenceCost))
239  {
240  if (j == states.size() - 1) // if there is a straight line between i and the last point, we are
241  // done
242  return result;
243  break;
244  }
245 
246  // Check if the shortcut i-j is better than the current path between i and j
247  if (obj_->isCostBetterThan(shortcutCost, alongPath))
248  {
249  // The shortcut is better than the current path, so remove the nodes in between
250  if (freeStates_)
251  for (std::size_t k = i + 1; k < j; ++k)
252  si->freeState(states[k]);
253  states.erase(states.begin() + i + 1, states.begin() + j);
254 
255  // Add intermediate states between i and j
256  double dist = si->distance(states[i], states[j]);
257  if (dist > delta)
258  {
259  std::size_t numIntermediateStates = (int)(floor(dist / delta));
260  double t = 1.0 / (numIntermediateStates + 1);
261  for (std::size_t k = 0; k < numIntermediateStates; ++k)
262  {
263  base::State *newState = si->allocState();
264  si->getStateSpace()->interpolate(states[i], states[i + 1 + k], t * (k + 1), newState);
265  states.insert(states.begin() + i + 1 + k, newState);
266  }
267  }
268 
269  // Update the cumulative costs
270  costs.resize(states.size(), obj_->identityCost());
271  for (std::size_t k = i + 1; k < costs.size(); ++k)
272  {
273  costs[k] = obj_->combineCosts(costs[k - 1], obj_->motionCost(states[k - 1], states[k]));
274  }
275  result = true;
276 
277  if (j == states.size() - 1) // if we just made a shortcut between i and the last point, we are done
278  return result;
279 
280  // Set i to -1 so that it starts again at the first node
281  i = -1;
282 
283  // Break out of j loop
284  break;
285  }
286  }
287  }
288  }
289 
290  return result;
291 }
292 
293 bool ompl::geometric::PathSimplifier::partialShortcutPath(PathGeometric &path, unsigned int maxSteps,
294  unsigned int maxEmptySteps, double rangeRatio,
295  double snapToVertex)
296 {
297  if (path.getStateCount() < 3)
298  return false;
299 
300  if (maxSteps == 0)
301  maxSteps = path.getStateCount();
302 
303  if (maxEmptySteps == 0)
304  maxEmptySteps = path.getStateCount();
305 
306  const base::SpaceInformationPtr &si = path.getSpaceInformation();
307  std::vector<base::State *> &states = path.getStates();
308 
309  // costs[i] contains the cumulative cost of the path up to and including state i
310  std::vector<base::Cost> costs(states.size(), obj_->identityCost());
311  std::vector<double> dists(states.size(), 0.0);
312  for (unsigned int i = 1; i < costs.size(); ++i)
313  {
314  costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
315  dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);
316  }
317  // Sampled states closer than 'threshold' distance to any existing state in the path
318  // are snapped to the close state
319  double threshold = dists.back() * snapToVertex;
320  // The range (distance) of a single connection that will be attempted
321  double rd = rangeRatio * dists.back();
322 
323  base::State *temp0 = si->allocState();
324  base::State *temp1 = si->allocState();
325  bool result = false;
326  unsigned int nochange = 0;
327  // Attempt shortcutting maxSteps times or when no improvement is found after
328  // maxEmptySteps attempts, whichever comes first
329  for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
330  {
331  // Sample a random point anywhere along the path
332  base::State *s0 = nullptr;
333  int index0 = -1;
334  double t0 = 0.0;
335  double distTo0 = rng_.uniformReal(0.0, dists.back()); // sample a random point (p0) along the path
336  auto pit =
337  std::lower_bound(dists.begin(), dists.end(), distTo0); // find the NEXT waypoint after the random point
338  int pos0 = pit == dists.end() ? dists.size() - 1 :
339  pit - dists.begin(); // get the index of the NEXT waypoint after the point
340 
341  if (pos0 == 0 || dists[pos0] - distTo0 < threshold) // snap to the NEXT waypoint
342  index0 = pos0;
343  else
344  {
345  while (pos0 > 0 && distTo0 < dists[pos0])
346  --pos0;
347  if (distTo0 - dists[pos0] < threshold) // snap to the PREVIOUS waypoint
348  index0 = pos0;
349  }
350 
351  // Sample a random point within rd distance of the previously sampled point
352  base::State *s1 = nullptr;
353  int index1 = -1;
354  double t1 = 0.0;
355  double distTo1 =
356  rng_.uniformReal(std::max(0.0, distTo0 - rd),
357  std::min(distTo0 + rd, dists.back())); // sample a random point (distTo1) near s0
358  pit = std::lower_bound(dists.begin(), dists.end(), distTo1); // find the NEXT waypoint after the random point
359  int pos1 = pit == dists.end() ? dists.size() - 1 :
360  pit - dists.begin(); // get the index of the NEXT waypoint after the point
361 
362  if (pos1 == 0 || dists[pos1] - distTo1 < threshold) // snap to the NEXT waypoint
363  index1 = pos1;
364  else
365  {
366  while (pos1 > 0 && distTo1 < dists[pos1])
367  --pos1;
368  if (distTo1 - dists[pos1] < threshold) // snap to the PREVIOUS waypoint
369  index1 = pos1;
370  }
371 
372  // Don't waste time on points that are on the same path segment
373  if (pos0 == pos1 || index0 == pos1 || index1 == pos0 || pos0 + 1 == index1 || pos1 + 1 == index0 ||
374  (index0 >= 0 && index1 >= 0 && abs(index0 - index1) < 2))
375  continue;
376 
377  // Get the state pointer for costTo0
378  if (index0 >= 0)
379  {
380  s0 = states[index0];
381  }
382  else
383  {
384  t0 = (distTo0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
385  si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
386  s0 = temp0;
387  }
388 
389  // Get the state pointer for costTo1
390  if (index1 >= 0)
391  {
392  s1 = states[index1];
393  }
394  else
395  {
396  t1 = (distTo1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
397  si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
398  s1 = temp1;
399  }
400 
401  // Check for validity between s0 and s1
402  if (si->checkMotion(s0, s1))
403  {
404  if (pos0 > pos1)
405  {
406  std::swap(pos0, pos1);
407  std::swap(index0, index1);
408  std::swap(s0, s1);
409  std::swap(t0, t1);
410  }
411 
412  // Now that states are in the right order, make sure the cost actually decreases.
413  base::Cost s0PartialCost = (index0 >= 0) ? obj_->identityCost() : obj_->motionCost(s0, states[pos0 + 1]);
414  base::Cost s1PartialCost = (index1 >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos1], s1);
415  base::Cost alongPath = s0PartialCost;
416  int posTemp = pos0 + 1;
417  while (posTemp < pos1)
418  {
419  alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
420  posTemp++;
421  }
422  alongPath = obj_->combineCosts(alongPath, s1PartialCost);
423  if (obj_->isCostBetterThan(alongPath, obj_->motionCost(s0, s1)))
424  {
425  // The cost along the path from state 0 to 1 is better than the straight line motion cost between the
426  // two.
427  continue;
428  }
429  // Otherwise, shortcut cost is better!
430 
431  // Modify the path with the new, shorter result
432  if (index0 < 0 && index1 < 0)
433  {
434  if (pos0 + 1 == pos1)
435  {
436  si->copyState(states[pos1], s0);
437  states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
438  }
439  else
440  {
441  if (freeStates_)
442  for (int j = pos0 + 2; j < pos1; ++j)
443  si->freeState(states[j]);
444  si->copyState(states[pos0 + 1], s0);
445  si->copyState(states[pos1], s1);
446  states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
447  }
448  }
449  else if (index0 >= 0 && index1 >= 0)
450  {
451  if (freeStates_)
452  for (int j = index0 + 1; j < index1; ++j)
453  si->freeState(states[j]);
454  states.erase(states.begin() + index0 + 1, states.begin() + index1);
455  }
456  else if (index0 < 0 && index1 >= 0)
457  {
458  if (freeStates_)
459  for (int j = pos0 + 2; j < index1; ++j)
460  si->freeState(states[j]);
461  si->copyState(states[pos0 + 1], s0);
462  states.erase(states.begin() + pos0 + 2, states.begin() + index1);
463  }
464  else if (index0 >= 0 && index1 < 0)
465  {
466  if (freeStates_)
467  for (int j = index0 + 1; j < pos1; ++j)
468  si->freeState(states[j]);
469  si->copyState(states[pos1], s1);
470  states.erase(states.begin() + index0 + 1, states.begin() + pos1);
471  }
472 
473  // fix the helper variables
474  dists.resize(states.size(), 0.0);
475  costs.resize(states.size(), obj_->identityCost());
476  for (unsigned int j = pos0 + 1; j < costs.size(); ++j)
477  {
478  costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
479  dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
480  }
481  threshold = dists.back() * snapToVertex;
482  rd = rangeRatio * dists.back();
483  result = true;
484  nochange = 0;
485  }
486  }
487 
488  si->freeState(temp1);
489  si->freeState(temp0);
490  return result;
491 }
492 
493 bool ompl::geometric::PathSimplifier::perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps,
494  unsigned int maxEmptySteps, double snapToVertex)
495 {
496  if (maxSteps == 0)
497  maxSteps = path.getStateCount();
498 
499  if (maxEmptySteps == 0)
500  maxEmptySteps = path.getStateCount();
501 
502  const base::SpaceInformationPtr &si = path.getSpaceInformation();
503  std::vector<base::State *> &states = path.getStates();
504 
505  std::vector<double> dists(states.size(), 0.0);
506  for (unsigned int i = 1; i < dists.size(); i++)
507  dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);
508 
509  std::vector<std::tuple<double, base::Cost, unsigned int>> distCostIndices;
510  for (unsigned int i = 0; i < states.size() - 1; i++)
511  distCostIndices.emplace_back(si->distance(states[i], states[i + 1]), obj_->motionCost(states[i], states[i + 1]),
512  i);
513 
514  // Sort so highest costs are first
515  std::sort(distCostIndices.begin(), distCostIndices.end(),
516  [this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
517  return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
518  });
519 
520  double threshold = dists.back() * snapToVertex;
521 
522  bool result = false;
523  unsigned int nochange = 0;
524 
525  base::StateSamplerPtr sampler = si->allocStateSampler();
526 
527  base::State *perturb_state = si->allocState();
528  base::State *before_state = si->allocState();
529  base::State *after_state = si->allocState();
530  base::State *new_state = si->allocState();
531 
532  // Attempt perturbing maxSteps times or when no improvement is found after
533  // maxEmptySteps attempts, whichever comes first.
534  for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; i++, nochange++)
535  {
536  // select a configuration on the path, biased towards high cost segments.
537  // Get a random real from 0 to the path length, biased towards 0.
538  double costBias = -1 * rng_.halfNormalReal(-dists.back(), 0.0, 20.0);
539  unsigned int z = 0;
540  if (costBias >= dists.back())
541  {
542  z = distCostIndices.size() - 1;
543  costBias = std::get<0>(distCostIndices[z]);
544  }
545  else
546  {
547  // Using our sorted cost vector, find the segment we want to use.
548  while (costBias - std::get<0>(distCostIndices[z]) > std::numeric_limits<double>::epsilon())
549  {
550  costBias -= std::get<0>(distCostIndices[z]);
551  z++;
552  }
553  }
554 
555  int pos, pos_before, pos_after;
556  double distTo = dists[std::get<2>(distCostIndices[z])] + costBias;
557  selectAlongPath(dists, states, distTo, threshold, perturb_state, pos);
558 
559  // Get before state and after state, that are around stepsize/2 on either side of perturb state.
560  int index_before = selectAlongPath(dists, states, distTo - stepSize / 2.0, threshold, before_state, pos_before);
561  int index_after = selectAlongPath(dists, states, distTo + stepSize / 2.0, threshold, after_state, pos_after);
562 
563  if (index_before >= 0 && index_after >= 0 && index_before == index_after)
564  {
565  continue;
566  }
567 
568  // Pick a random direction to extend and take a stepSize step in that direction.
569  sampler->sampleUniform(new_state);
570  double dist = si->distance(perturb_state, new_state);
571  si->getStateSpace()->interpolate(perturb_state, new_state, stepSize / dist, new_state);
572 
573  // Check for validity of the new path to the new state.
574  if (si->checkMotion(before_state, new_state) && si->checkMotion(new_state, after_state))
575  {
576  // Now check for improved cost. Get the original cost along the path.
577  base::Cost alongPath;
578  if (pos_before == pos_after)
579  {
580  alongPath = obj_->motionCost(before_state, after_state);
581  }
582  else
583  {
584  // The partial cost from before_state to the first waypoint.
585  alongPath =
586  (index_before >= 0) ? obj_->identityCost() : obj_->motionCost(before_state, states[pos_before + 1]);
587  int posTemp = (index_before >= 0) ? index_before : pos_before + 1;
588  while (posTemp < pos_after)
589  {
590  alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
591  posTemp++;
592  }
593  base::Cost afterPartialCost =
594  (index_after >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos_after], after_state);
595  alongPath = obj_->combineCosts(alongPath, afterPartialCost);
596  }
597 
598  base::Cost newCost =
599  obj_->combineCosts(obj_->motionCost(before_state, new_state), obj_->motionCost(new_state, after_state));
600  if (obj_->isCostBetterThan(alongPath, newCost) || obj_->isCostEquivalentTo(alongPath, newCost))
601  {
602  // Cost along the current path is better than the perturbed path.
603  continue;
604  }
605 
606  // Modify the path with the new state.
607  if (index_before < 0 && index_after < 0)
608  {
609  if (pos_before == pos_after)
610  {
611  // Insert all 3 states; new_state goes before after_state, and before_state
612  // goes before new_state.
613  states.insert(states.begin() + pos_before + 1, si->cloneState(after_state));
614  states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
615  states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
616  }
617  else if (pos_before + 1 == pos_after)
618  {
619  si->copyState(states[pos_after], before_state);
620  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
621  states.insert(states.begin() + pos_after + 1, si->cloneState(new_state));
622  }
623  else if (pos_before + 2 == pos_after)
624  {
625  si->copyState(states[pos_before + 1], before_state);
626  si->copyState(states[pos_after], new_state);
627  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
628  }
629  else
630  {
631  si->copyState(states[pos_before + 1], before_state);
632  si->copyState(states[pos_before + 2], new_state);
633  si->copyState(states[pos_before + 3], after_state);
634  if (freeStates_)
635  for (int j = pos_before + 4; j < pos_after + 1; ++j)
636  si->freeState(states[j]);
637  states.erase(states.begin() + pos_before + 4, states.begin() + pos_after + 1);
638  }
639  }
640  else if (index_before >= 0 && index_after >= 0)
641  {
642  states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
643  if (freeStates_)
644  for (int j = index_before + 2; j < index_after + 1; ++j)
645  si->freeState(states[j]);
646  states.erase(states.begin() + index_before + 2, states.begin() + index_after + 1);
647  }
648  else if (index_before < 0 && index_after >= 0)
649  {
650  if (index_after > pos_before + 1)
651  {
652  si->copyState(states[pos_before + 1], before_state);
653  states.insert(states.begin() + pos_before + 2, si->cloneState(new_state));
654  if (freeStates_)
655  for (int j = pos_before + 3; j < index_after + 1; ++j)
656  si->freeState(states[j]);
657  states.erase(states.begin() + pos_before + 3, states.begin() + index_after + 1);
658  }
659  else
660  {
661  states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
662  states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
663  }
664  }
665  else if (index_before >= 0 && index_after < 0)
666  {
667  if (pos_after > index_before)
668  {
669  si->copyState(states[pos_after], new_state);
670  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
671  if (freeStates_)
672  for (int j = index_before + 1; j < pos_after; ++j)
673  si->freeState(states[j]);
674  states.erase(states.begin() + index_before + 1, states.begin() + pos_after);
675  }
676  else
677  {
678  states.insert(states.begin() + index_before + 1, si->cloneState(after_state));
679  states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
680  }
681  }
682 
683  // fix the helper variables
684  dists.resize(states.size(), 0.0);
685  for (unsigned int j = pos_before + 1; j < dists.size(); ++j)
686  {
687  dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
688  }
689  distCostIndices.clear();
690  for (unsigned int i = 0; i < states.size() - 1; i++)
691  distCostIndices.emplace_back(si->distance(states[i], states[i + 1]),
692  obj_->motionCost(states[i], states[i + 1]), i);
693 
694  // Sort so highest costs are first
695  std::sort(
696  distCostIndices.begin(), distCostIndices.end(),
697  [this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
698  return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
699  });
700  threshold = dists.back() * snapToVertex;
701  result = true;
702  nochange = 0;
703  }
704  }
705  si->freeState(perturb_state);
706  si->freeState(before_state);
707  si->freeState(after_state);
708  si->freeState(new_state);
709 
710  return result;
711 }
712 
713 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps,
714  unsigned int maxEmptySteps)
715 {
716  if (path.getStateCount() < 3)
717  return false;
718 
719  if (maxSteps == 0)
720  maxSteps = path.getStateCount();
721 
722  if (maxEmptySteps == 0)
723  maxEmptySteps = path.getStateCount();
724 
725  const base::SpaceInformationPtr &si = path.getSpaceInformation();
726  std::vector<base::State *> &states = path.getStates();
727 
728  // compute pair-wise distances in path (construct only half the matrix)
729  std::map<std::pair<const base::State *, const base::State *>, double> distances;
730  for (unsigned int i = 0; i < states.size(); ++i)
731  for (unsigned int j = i + 2; j < states.size(); ++j)
732  distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
733 
734  bool result = false;
735  unsigned int nochange = 0;
736  for (unsigned int s = 0; s < maxSteps && nochange < maxEmptySteps; ++s, ++nochange)
737  {
738  // find closest pair of points
739  double minDist = std::numeric_limits<double>::infinity();
740  int p1 = -1;
741  int p2 = -1;
742  for (unsigned int i = 0; i < states.size(); ++i)
743  for (unsigned int j = i + 2; j < states.size(); ++j)
744  {
745  double d = distances[std::make_pair(states[i], states[j])];
746  if (d < minDist)
747  {
748  minDist = d;
749  p1 = i;
750  p2 = j;
751  }
752  }
753 
754  if (p1 >= 0 && p2 >= 0)
755  {
756  if (si->checkMotion(states[p1], states[p2]))
757  {
758  if (freeStates_)
759  for (int i = p1 + 1; i < p2; ++i)
760  si->freeState(states[i]);
761  states.erase(states.begin() + p1 + 1, states.begin() + p2);
762  result = true;
763  nochange = 0;
764  }
765  else
766  distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
767  }
768  else
769  break;
770  }
771  return result;
772 }
773 
774 bool ompl::geometric::PathSimplifier::simplifyMax(PathGeometric &path)
775 {
776  ompl::base::PlannerTerminationCondition neverTerminate = base::plannerNonTerminatingCondition();
777  return simplify(path, neverTerminate);
778 }
779 
780 bool ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime, bool atLeastOnce)
781 {
782  return simplify(path, base::timedPlannerTerminationCondition(maxTime), atLeastOnce);
783 }
784 
785 bool ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
786  bool atLeastOnce)
787 {
788  if (path.getStateCount() < 3)
789  return true;
790 
791  bool tryMore = true, valid = true;
792  while ((ptc == false || atLeastOnce) && tryMore)
793  {
794  // if the space is metric, we can do some additional smoothing
795  if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
796  {
797  bool metricTryMore = true;
798  unsigned int times = 0;
799  do
800  {
801  bool shortcut = partialShortcutPath(path); // split path segments, not just vertices
802  bool better_goal =
803  gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
804 
805  metricTryMore = shortcut || better_goal;
806  } while ((ptc == false || atLeastOnce) && ++times <= 5 && metricTryMore);
807 
808  // smooth the path with BSpline interpolation
809  if (ptc == false || atLeastOnce)
810  smoothBSpline(path, 3, path.length() / 100.0);
811 
812  if (ptc == false || atLeastOnce)
813  {
814  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
815  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
816  if (!p.second)
817  {
818  valid = false;
819  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
820  }
821  else if (!p.first)
822  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but "
823  "it was "
824  "successfully fixed.");
825  }
826  }
827 
828  // try a randomized step of connecting vertices
829  if (ptc == false || atLeastOnce)
830  tryMore = reduceVertices(path);
831 
832  // try to collapse close-by vertices
833  if (ptc == false || atLeastOnce)
834  collapseCloseVertices(path);
835 
836  // try to reduce verices some more, if there is any point in doing so
837  unsigned int times = 0;
838  while ((ptc == false || atLeastOnce) && tryMore && ++times <= 5)
839  tryMore = reduceVertices(path);
840 
841  if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
842  {
843  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
844  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
845  if (!p.second)
846  {
847  valid = false;
848  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
849  }
850  else if (!p.first)
851  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it "
852  "was "
853  "successfully fixed.");
854  }
855 
856  atLeastOnce = false;
857  }
858  return valid || path.check();
859 }
860 
861 bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts,
862  double rangeRatio, double snapToVertex)
863 {
864  return findBetterGoal(path, base::timedPlannerTerminationCondition(maxTime), samplingAttempts, rangeRatio,
865  snapToVertex);
866 }
867 
868 bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
869  unsigned int samplingAttempts, double rangeRatio,
870  double snapToVertex)
871 {
872  if (path.getStateCount() < 2)
873  return false;
874 
875  if (!gsr_)
876  {
877  OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
878  return false;
879  }
880 
881  unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
882  unsigned int failedTries = 0;
883  bool betterGoal = false;
884 
885  const base::StateSpacePtr &ss = si_->getStateSpace();
886  std::vector<base::State *> &states = path.getStates();
887 
888  // dists[i] contains the cumulative length of the path up to and including state i
889  std::vector<base::Cost> costs(states.size(), obj_->identityCost());
890  std::vector<double> dists(states.size(), 0.0);
891  for (unsigned int i = 1; i < dists.size(); ++i)
892  {
893  costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
894  dists[i] = dists[i - 1] + si_->distance(states[i - 1], states[i]);
895  if (dists[i] < 0)
896  {
897  OMPL_WARN("%s: Somehow computed negative distance!.", "PathSimplifier::findBetterGoal");
898  return false;
899  }
900  }
901 
902  // Sampled states closer than 'threshold' distance to any existing state in the path
903  // are snapped to the close state
904  double threshold = dists.back() * snapToVertex;
905  // The range (distance) of a single connection that will be attempted
906  double rd = rangeRatio * dists.back();
907 
908  base::State *temp = si_->allocState();
909  base::State *tempGoal = si_->allocState();
910 
911  while (!ptc && failedTries++ < maxGoals && !betterGoal)
912  {
913  gsr_->sampleGoal(tempGoal);
914 
915  // Goal state is not compatible with the start state
916  if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
917  continue;
918 
919  unsigned int numSamples = 0;
920  while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
921  {
922  // sample a state within rangeRatio
923  double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0),
924  dists.back()); // Sample a random point within rd of the end of the path
925 
926  auto end = std::lower_bound(dists.begin(), dists.end(), t);
927  auto start = end;
928  while (start != dists.begin() && *start >= t)
929  start -= 1;
930 
931  unsigned int startIndex = start - dists.begin();
932  unsigned int endIndex = end - dists.begin();
933 
934  // Snap the random point to the nearest vertex, if within the threshold
935  if (t - (*start) < threshold) // snap to the starting waypoint
936  endIndex = startIndex;
937  if ((*end) - t < threshold) // snap to the ending waypoint
938  startIndex = endIndex;
939 
940  // Compute the state value and the accumulated cost up to that state
941  base::Cost costToCome = costs[startIndex];
942  base::State *state;
943  if (startIndex == endIndex)
944  {
945  state = states[startIndex];
946  }
947  else
948  {
949  double tSeg = (t - (*start)) / (*end - *start);
950  ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
951  state = temp;
952 
953  costToCome = obj_->combineCosts(costToCome, obj_->motionCost(states[startIndex], state));
954  }
955 
956  base::Cost costToGo = obj_->motionCost(state, tempGoal);
957  base::Cost candidateCost = obj_->combineCosts(costToCome, costToGo);
958 
959  // Make sure we improve before attempting validation
960  if (obj_->isCostBetterThan(candidateCost, costs.back()) && si_->checkMotion(state, tempGoal))
961  {
962  // insert the new states
963  if (startIndex == endIndex)
964  {
965  // new intermediate state
966  si_->copyState(states[startIndex], state);
967  // new goal state
968  si_->copyState(states[startIndex + 1], tempGoal);
969 
970  if (freeStates_)
971  {
972  for (size_t i = startIndex + 2; i < states.size(); ++i)
973  si_->freeState(states[i]);
974  }
975  states.erase(states.begin() + startIndex + 2, states.end());
976  }
977  else
978  {
979  // overwriting the end of the segment with the new state
980  si_->copyState(states[endIndex], state);
981  if (endIndex == states.size() - 1)
982  {
983  path.append(tempGoal);
984  }
985  else
986  {
987  // adding goal new goal state
988  si_->copyState(states[endIndex + 1], tempGoal);
989  if (freeStates_)
990  {
991  for (size_t i = endIndex + 2; i < states.size(); ++i)
992  si_->freeState(states[i]);
993  }
994  states.erase(states.begin() + endIndex + 2, states.end());
995  }
996  }
997 
998  // fix the helper variables
999  costs.resize(states.size(), obj_->identityCost());
1000  dists.resize(states.size(), 0.0);
1001  for (unsigned int j = std::max(1u, startIndex); j < dists.size(); ++j)
1002  {
1003  costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
1004  dists[j] = dists[j - 1] + si_->distance(states[j - 1], states[j]);
1005  }
1006 
1007  betterGoal = true;
1008  }
1009  }
1010  }
1011 
1012  si_->freeState(temp);
1013  si_->freeState(tempGoal);
1014 
1015  return betterGoal;
1016 }
1017 
1018 int ompl::geometric::PathSimplifier::selectAlongPath(std::vector<double> dists, std::vector<base::State *> states,
1019  double distTo, double threshold, base::State *select_state,
1020  int &pos)
1021 {
1022  if (distTo < 0)
1023  distTo = 0;
1024  else if (distTo > dists.back())
1025  distTo = dists.back();
1026 
1027  int index = -1;
1028  auto pit = std::lower_bound(dists.begin(), dists.end(), distTo);
1029  pos = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
1030 
1031  if (pos == 0 || dists[pos] - distTo < threshold)
1032  index = pos;
1033  else
1034  {
1035  while (pos > 0 && distTo < dists[pos])
1036  --pos;
1037  if (distTo - dists[pos] < threshold)
1038  index = pos;
1039  }
1040 
1041  if (index >= 0)
1042  {
1043  si_->copyState(select_state, states[index]);
1044  return index;
1045  }
1046  else
1047  {
1048  double t = (distTo - dists[pos]) / (dists[pos + 1] - dists[pos]);
1049  si_->getStateSpace()->interpolate(states[pos], states[pos + 1], t, select_state);
1050  return -1;
1051  }
1052 }
ompl::geometric::PathSimplifier::obj_
base::OptimizationObjectivePtr obj_
The optimization objective to use when making improvements. Will be used on all methods except reduce...
Definition: PathSimplifier.h:369
ompl::geometric::PathSimplifier::ropeShortcutPath
bool ropeShortcutPath(PathGeometric &path, double delta=1.0, double equivalenceTolerance=0.1)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
Definition: PathSimplifier.cpp:187
ompl::geometric::PathSimplifier::gsr_
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
Definition: PathSimplifier.h:365
ompl::geometric::PathSimplifier::reduceVertices
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...
Definition: PathSimplifier.cpp:124
ompl::geometric::PathSimplifier::collapseCloseVertices
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...
Definition: PathSimplifier.cpp:713
ompl::base::timedPlannerTerminationCondition
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
Definition: PlannerTerminationCondition.cpp:216
ompl::base::State
Definition of an abstract state.
Definition: State.h:113
ompl::geometric::PathSimplifier::perturbPath
bool perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double snapToVertex=0.005)
Given a path, attempt to improve the cost by randomly perturbing a randomly selected point on the pat...
Definition: PathSimplifier.cpp:493
ompl::geometric::PathSimplifier::partialShortcutPath
bool partialShortcutPath(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...
Definition: PathSimplifier.cpp:293
ompl::geometric::PathSimplifier::smoothBSpline
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).
Definition: PathSimplifier.cpp:80
ompl::base::Cost
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
ompl::geometric::PathSimplifier::simplify
bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce=true)
Run simplification algorithms on the path for at most maxTime seconds, and at least once if atLeastOn...
Definition: PathSimplifier.cpp:780
ompl::geometric::PathSimplifier::PathSimplifier
PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal=ompl::base::GoalPtr(), const base::OptimizationObjectivePtr &obj=nullptr)
Create an instance for a specified space information. Optionally, a GoalSampleableRegion may be passe...
Definition: PathSimplifier.cpp:48
ompl::geometric::PathGeometric
Definition of a geometric path.
Definition: PathGeometric.h:97
ompl::base::PlannerTerminationCondition
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:127
ompl::geometric::PathSimplifier::findBetterGoal
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...
Definition: PathSimplifier.cpp:861
OMPL_WARN
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
ompl::geometric::PathSimplifier::simplifyMax
bool simplifyMax(PathGeometric &path)
Given a path, attempt to remove vertices from it while keeping the path valid. Then,...
Definition: PathSimplifier.cpp:774
ompl::geometric::PathSimplifier::si_
base::SpaceInformationPtr si_
The space information this path simplifier uses.
Definition: PathSimplifier.h:362
ompl::magic::MAX_VALID_SAMPLE_ATTEMPTS
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...
Definition: MagicConstants.h:154
ompl::base::plannerNonTerminatingCondition
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met.
Definition: PlannerTerminationCondition.cpp:182
ompl::geometric::PathSimplifier::freeStates
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification.
Definition: PathSimplifier.cpp:69
OMPL_DEBUG
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70