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 "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 
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 
70 {
71  return freeStates_;
72 }
73 
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 
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 
188  unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
189 {
190  if (path.getStateCount() < 3)
191  return false;
192 
193  if (maxSteps == 0)
194  maxSteps = path.getStateCount();
195 
196  if (maxEmptySteps == 0)
197  maxEmptySteps = path.getStateCount();
198 
199  const base::SpaceInformationPtr &si = path.getSpaceInformation();
200  std::vector<base::State *> &states = path.getStates();
201 
202  // costs[i] contains the cumulative cost of the path up to and including state i
203  std::vector<base::Cost> costs(states.size(), obj_->identityCost());
204  std::vector<double> dists(states.size(), 0.0);
205  for (unsigned int i = 1; i < costs.size(); ++i)
206  {
207  costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
208  dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);
209  }
210  // Sampled states closer than 'threshold' distance to any existing state in the path
211  // are snapped to the close state
212  double threshold = dists.back() * snapToVertex;
213  // The range (distance) of a single connection that will be attempted
214  double rd = rangeRatio * dists.back();
215 
216  base::State *temp0 = si->allocState();
217  base::State *temp1 = si->allocState();
218  bool result = false;
219  unsigned int nochange = 0;
220  // Attempt shortcutting maxSteps times or when no improvement is found after
221  // maxEmptySteps attempts, whichever comes first
222  for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
223  {
224  // Sample a random point anywhere along the path
225  base::State *s0 = nullptr;
226  int index0 = -1;
227  double t0 = 0.0;
228  double distTo0 = rng_.uniformReal(0.0, dists.back()); // sample a random point (p0) along the path
229  auto pit =
230  std::lower_bound(dists.begin(), dists.end(), distTo0); // find the NEXT waypoint after the random point
231  int pos0 = pit == dists.end() ? dists.size() - 1 :
232  pit - dists.begin(); // get the index of the NEXT waypoint after the point
233 
234  if (pos0 == 0 || dists[pos0] - distTo0 < threshold) // snap to the NEXT waypoint
235  index0 = pos0;
236  else
237  {
238  while (pos0 > 0 && distTo0 < dists[pos0])
239  --pos0;
240  if (distTo0 - dists[pos0] < threshold) // snap to the PREVIOUS waypoint
241  index0 = pos0;
242  }
243 
244  // Sample a random point within rd distance of the previously sampled point
245  base::State *s1 = nullptr;
246  int index1 = -1;
247  double t1 = 0.0;
248  double distTo1 =
249  rng_.uniformReal(std::max(0.0, distTo0 - rd),
250  std::min(distTo0 + rd, dists.back())); // sample a random point (distTo1) near s0
251  pit = std::lower_bound(dists.begin(), dists.end(), distTo1); // find the NEXT waypoint after the random point
252  int pos1 = pit == dists.end() ? dists.size() - 1 :
253  pit - dists.begin(); // get the index of the NEXT waypoint after the point
254 
255  if (pos1 == 0 || dists[pos1] - distTo1 < threshold) // snap to the NEXT waypoint
256  index1 = pos1;
257  else
258  {
259  while (pos1 > 0 && distTo1 < dists[pos1])
260  --pos1;
261  if (distTo1 - dists[pos1] < threshold) // snap to the PREVIOUS waypoint
262  index1 = pos1;
263  }
264 
265  // Don't waste time on points that are on the same path segment
266  if (pos0 == pos1 || index0 == pos1 || index1 == pos0 || pos0 + 1 == index1 || pos1 + 1 == index0 ||
267  (index0 >= 0 && index1 >= 0 && abs(index0 - index1) < 2))
268  continue;
269 
270  // Get the state pointer for costTo0
271  if (index0 >= 0)
272  {
273  s0 = states[index0];
274  }
275  else
276  {
277  t0 = (distTo0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
278  si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
279  s0 = temp0;
280  }
281 
282  // Get the state pointer for costTo1
283  if (index1 >= 0)
284  {
285  s1 = states[index1];
286  }
287  else
288  {
289  t1 = (distTo1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
290  si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
291  s1 = temp1;
292  }
293 
294  // Check for validity between s0 and s1
295  if (si->checkMotion(s0, s1))
296  {
297  if (pos0 > pos1)
298  {
299  std::swap(pos0, pos1);
300  std::swap(index0, index1);
301  std::swap(s0, s1);
302  std::swap(t0, t1);
303  }
304 
305  // Now that states are in the right order, make sure the cost actually decreases.
306  base::Cost s0PartialCost = (index0 >= 0) ? obj_->identityCost() : obj_->motionCost(s0, states[pos0 + 1]);
307  base::Cost s1PartialCost = (index1 >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos1], s1);
308  base::Cost alongPath = s0PartialCost;
309  int posTemp = pos0 + 1;
310  while (posTemp < pos1)
311  {
312  alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
313  posTemp++;
314  }
315  alongPath = obj_->combineCosts(alongPath, s1PartialCost);
316  if (obj_->isCostBetterThan(alongPath, obj_->motionCost(s0, s1)))
317  {
318  // The cost along the path from state 0 to 1 is better than the straight line motion cost between the
319  // two.
320  continue;
321  }
322  // Otherwise, shortcut cost is better!
323 
324  // Modify the path with the new, shorter result
325  if (index0 < 0 && index1 < 0)
326  {
327  if (pos0 + 1 == pos1)
328  {
329  si->copyState(states[pos1], s0);
330  states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
331  }
332  else
333  {
334  if (freeStates_)
335  for (int j = pos0 + 2; j < pos1; ++j)
336  si->freeState(states[j]);
337  si->copyState(states[pos0 + 1], s0);
338  si->copyState(states[pos1], s1);
339  states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
340  }
341  }
342  else if (index0 >= 0 && index1 >= 0)
343  {
344  if (freeStates_)
345  for (int j = index0 + 1; j < index1; ++j)
346  si->freeState(states[j]);
347  states.erase(states.begin() + index0 + 1, states.begin() + index1);
348  }
349  else if (index0 < 0 && index1 >= 0)
350  {
351  if (freeStates_)
352  for (int j = pos0 + 2; j < index1; ++j)
353  si->freeState(states[j]);
354  si->copyState(states[pos0 + 1], s0);
355  states.erase(states.begin() + pos0 + 2, states.begin() + index1);
356  }
357  else if (index0 >= 0 && index1 < 0)
358  {
359  if (freeStates_)
360  for (int j = index0 + 1; j < pos1; ++j)
361  si->freeState(states[j]);
362  si->copyState(states[pos1], s1);
363  states.erase(states.begin() + index0 + 1, states.begin() + pos1);
364  }
365 
366  // fix the helper variables
367  dists.resize(states.size(), 0.0);
368  costs.resize(states.size(), obj_->identityCost());
369  for (unsigned int j = pos0 + 1; j < costs.size(); ++j)
370  {
371  costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
372  dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
373  }
374  threshold = dists.back() * snapToVertex;
375  rd = rangeRatio * dists.back();
376  result = true;
377  nochange = 0;
378  }
379  }
380 
381  si->freeState(temp1);
382  si->freeState(temp0);
383  return result;
384 }
385 
386 bool ompl::geometric::PathSimplifier::perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps,
387  unsigned int maxEmptySteps, double snapToVertex)
388 {
389  if (maxSteps == 0)
390  maxSteps = path.getStateCount();
391 
392  if (maxEmptySteps == 0)
393  maxEmptySteps = path.getStateCount();
394 
395  const base::SpaceInformationPtr &si = path.getSpaceInformation();
396  std::vector<base::State *> &states = path.getStates();
397 
398  std::vector<double> dists(states.size(), 0.0);
399  for (unsigned int i = 1; i < dists.size(); i++)
400  dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);
401 
402  std::vector<std::tuple<double, base::Cost, unsigned int>> distCostIndices;
403  for (unsigned int i = 0; i < states.size() - 1; i++)
404  distCostIndices.push_back(
405  std::make_tuple(si->distance(states[i], states[i + 1]), obj_->motionCost(states[i], states[i + 1]), i));
406 
407  // Sort so highest costs are first
408  std::sort(distCostIndices.begin(), distCostIndices.end(),
409  [this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
410  return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
411  });
412 
413  double threshold = dists.back() * snapToVertex;
414 
415  bool result = false;
416  unsigned int nochange = 0;
417 
418  base::StateSamplerPtr sampler = si->allocStateSampler();
419 
420  base::State *perturb_state = si->allocState();
421  base::State *before_state = si->allocState();
422  base::State *after_state = si->allocState();
423  base::State *new_state = si->allocState();
424 
425  // Attempt perturbing maxSteps times or when no improvement is found after
426  // maxEmptySteps attempts, whichever comes first.
427  for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; i++, nochange++)
428  {
429  // select a configuration on the path, biased towards high cost segments.
430  // Get a random real from 0 to the path length, biased towards 0.
431  double costBias = -1 * rng_.halfNormalReal(-dists.back(), 0.0, 20.0);
432  unsigned int z = 0;
433  if (costBias >= dists.back())
434  {
435  z = distCostIndices.size() - 1;
436  costBias = std::get<0>(distCostIndices[z]);
437  }
438  else
439  {
440  // Using our sorted cost vector, find the segment we want to use.
441  while (costBias - std::get<0>(distCostIndices[z]) > std::numeric_limits<double>::epsilon())
442  {
443  costBias -= std::get<0>(distCostIndices[z]);
444  z++;
445  }
446  }
447 
448  int pos, pos_before, pos_after;
449  double distTo = dists[std::get<2>(distCostIndices[z])] + costBias;
450  selectAlongPath(dists, states, distTo, threshold, perturb_state, pos);
451 
452  // Get before state and after state, that are around stepsize/2 on either side of perturb state.
453  int index_before = selectAlongPath(dists, states, distTo - stepSize / 2.0, threshold, before_state, pos_before);
454  int index_after = selectAlongPath(dists, states, distTo + stepSize / 2.0, threshold, after_state, pos_after);
455 
456  if (index_before >= 0 && index_after >= 0 && index_before == index_after)
457  {
458  continue;
459  }
460 
461  // Pick a random direction to extend and take a stepSize step in that direction.
462  sampler->sampleUniform(new_state);
463  double dist = si->distance(perturb_state, new_state);
464  si->getStateSpace()->interpolate(perturb_state, new_state, stepSize / dist, new_state);
465 
466  // Check for validity of the new path to the new state.
467  if (si->checkMotion(before_state, new_state) && si->checkMotion(new_state, after_state))
468  {
469  // Now check for improved cost. Get the original cost along the path.
470  base::Cost alongPath;
471  if (pos_before == pos_after)
472  {
473  alongPath = obj_->motionCost(before_state, after_state);
474  }
475  else
476  {
477  // The partial cost from before_state to the first waypoint.
478  alongPath =
479  (index_before >= 0) ? obj_->identityCost() : obj_->motionCost(before_state, states[pos_before + 1]);
480  int posTemp = (index_before >= 0) ? index_before : pos_before + 1;
481  while (posTemp < pos_after)
482  {
483  alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
484  posTemp++;
485  }
486  base::Cost afterPartialCost =
487  (index_after >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos_after], after_state);
488  alongPath = obj_->combineCosts(alongPath, afterPartialCost);
489  }
490 
491  base::Cost newCost =
492  obj_->combineCosts(obj_->motionCost(before_state, new_state), obj_->motionCost(new_state, after_state));
493  if (obj_->isCostBetterThan(alongPath, newCost) || obj_->isCostEquivalentTo(alongPath, newCost))
494  {
495  // Cost along the current path is better than the perturbed path.
496  continue;
497  }
498 
499  // Modify the path with the new state.
500  if (index_before < 0 && index_after < 0)
501  {
502  if (pos_before == pos_after)
503  {
504  // Insert all 3 states; new_state goes before after_state, and before_state
505  // goes before new_state.
506  states.insert(states.begin() + pos_before + 1, si->cloneState(after_state));
507  states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
508  states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
509  }
510  else if (pos_before + 1 == pos_after)
511  {
512  si->copyState(states[pos_after], before_state);
513  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
514  states.insert(states.begin() + pos_after + 1, si->cloneState(new_state));
515  }
516  else if (pos_before + 2 == pos_after)
517  {
518  si->copyState(states[pos_before + 1], before_state);
519  si->copyState(states[pos_after], new_state);
520  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
521  }
522  else
523  {
524  si->copyState(states[pos_before + 1], before_state);
525  si->copyState(states[pos_before + 2], new_state);
526  si->copyState(states[pos_before + 3], after_state);
527  if (freeStates_)
528  for (int j = pos_before + 4; j < pos_after + 1; ++j)
529  si->freeState(states[j]);
530  states.erase(states.begin() + pos_before + 4, states.begin() + pos_after + 1);
531  }
532  }
533  else if (index_before >= 0 && index_after >= 0)
534  {
535  states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
536  if (freeStates_)
537  for (int j = index_before + 2; j < index_after + 1; ++j)
538  si->freeState(states[j]);
539  states.erase(states.begin() + index_before + 2, states.begin() + index_after + 1);
540  }
541  else if (index_before < 0 && index_after >= 0)
542  {
543  if (index_after > pos_before + 1)
544  {
545  si->copyState(states[pos_before + 1], before_state);
546  states.insert(states.begin() + pos_before + 2, si->cloneState(new_state));
547  if (freeStates_)
548  for (int j = pos_before + 3; j < index_after + 1; ++j)
549  si->freeState(states[j]);
550  states.erase(states.begin() + pos_before + 3, states.begin() + index_after + 1);
551  }
552  else
553  {
554  states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
555  states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
556  }
557  }
558  else if (index_before >= 0 && index_after < 0)
559  {
560  if (pos_after > index_before)
561  {
562  si->copyState(states[pos_after], new_state);
563  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
564  if (freeStates_)
565  for (int j = index_before + 1; j < pos_after; ++j)
566  si->freeState(states[j]);
567  states.erase(states.begin() + index_before + 1, states.begin() + pos_after);
568  }
569  else
570  {
571  states.insert(states.begin() + index_before + 1, si->cloneState(after_state));
572  states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
573  }
574  }
575 
576  // fix the helper variables
577  dists.resize(states.size(), 0.0);
578  for (unsigned int j = pos_before + 1; j < dists.size(); ++j)
579  {
580  dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
581  }
582  distCostIndices.clear();
583  for (unsigned int i = 0; i < states.size() - 1; i++)
584  distCostIndices.push_back(std::make_tuple(si->distance(states[i], states[i + 1]),
585  obj_->motionCost(states[i], states[i + 1]), i));
586 
587  // Sort so highest costs are first
588  std::sort(
589  distCostIndices.begin(), distCostIndices.end(),
590  [this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
591  return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
592  });
593  threshold = dists.back() * snapToVertex;
594  result = true;
595  nochange = 0;
596  }
597  }
598  si->freeState(perturb_state);
599  si->freeState(before_state);
600  si->freeState(after_state);
601  si->freeState(new_state);
602 
603  return result;
604 }
605 
607  unsigned int maxEmptySteps)
608 {
609  if (path.getStateCount() < 3)
610  return false;
611 
612  if (maxSteps == 0)
613  maxSteps = path.getStateCount();
614 
615  if (maxEmptySteps == 0)
616  maxEmptySteps = path.getStateCount();
617 
618  const base::SpaceInformationPtr &si = path.getSpaceInformation();
619  std::vector<base::State *> &states = path.getStates();
620 
621  // compute pair-wise distances in path (construct only half the matrix)
622  std::map<std::pair<const base::State *, const base::State *>, double> distances;
623  for (unsigned int i = 0; i < states.size(); ++i)
624  for (unsigned int j = i + 2; j < states.size(); ++j)
625  distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
626 
627  bool result = false;
628  unsigned int nochange = 0;
629  for (unsigned int s = 0; s < maxSteps && nochange < maxEmptySteps; ++s, ++nochange)
630  {
631  // find closest pair of points
632  double minDist = std::numeric_limits<double>::infinity();
633  int p1 = -1;
634  int p2 = -1;
635  for (unsigned int i = 0; i < states.size(); ++i)
636  for (unsigned int j = i + 2; j < states.size(); ++j)
637  {
638  double d = distances[std::make_pair(states[i], states[j])];
639  if (d < minDist)
640  {
641  minDist = d;
642  p1 = i;
643  p2 = j;
644  }
645  }
646 
647  if (p1 >= 0 && p2 >= 0)
648  {
649  if (si->checkMotion(states[p1], states[p2]))
650  {
651  if (freeStates_)
652  for (int i = p1 + 1; i < p2; ++i)
653  si->freeState(states[i]);
654  states.erase(states.begin() + p1 + 1, states.begin() + p2);
655  result = true;
656  nochange = 0;
657  }
658  else
659  distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
660  }
661  else
662  break;
663  }
664  return result;
665 }
666 
668 {
670  return simplify(path, neverTerminate);
671 }
672 
673 bool ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime, bool atLeastOnce)
674 {
675  return simplify(path, base::timedPlannerTerminationCondition(maxTime), atLeastOnce);
676 }
677 
679  bool atLeastOnce)
680 {
681  if (path.getStateCount() < 3)
682  return true;
683 
684  bool tryMore = true, valid = true;
685  while ((ptc == false || atLeastOnce) && tryMore)
686  {
687  // if the space is metric, we can do some additional smoothing
688  if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
689  {
690  bool metricTryMore = true;
691  unsigned int times = 0;
692  do
693  {
694  bool shortcut = shortcutPath(path); // split path segments, not just vertices
695  bool better_goal =
696  gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
697 
698  metricTryMore = shortcut || better_goal;
699  } while ((ptc == false || atLeastOnce) && ++times <= 5 && metricTryMore);
700 
701  // smooth the path with BSpline interpolation
702  if (ptc == false || atLeastOnce)
703  smoothBSpline(path, 3, path.length() / 100.0);
704 
705  if (ptc == false || atLeastOnce)
706  {
707  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
708  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
709  if (!p.second)
710  {
711  valid = false;
712  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
713  }
714  else if (!p.first)
715  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but "
716  "it was "
717  "successfully fixed.");
718  }
719  }
720 
721  // try a randomized step of connecting vertices
722  if (ptc == false || atLeastOnce)
723  tryMore = reduceVertices(path);
724 
725  // try to collapse close-by vertices
726  if (ptc == false || atLeastOnce)
727  collapseCloseVertices(path);
728 
729  // try to reduce verices some more, if there is any point in doing so
730  unsigned int times = 0;
731  while ((ptc == false || atLeastOnce) && tryMore && ++times <= 5)
732  tryMore = reduceVertices(path);
733 
734  if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
735  {
736  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
737  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
738  if (!p.second)
739  {
740  valid = false;
741  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
742  }
743  else if (!p.first)
744  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it "
745  "was "
746  "successfully fixed.");
747  }
748 
749  atLeastOnce = false;
750  }
751  return valid || path.check();
752 }
753 
754 bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts,
755  double rangeRatio, double snapToVertex)
756 {
757  return findBetterGoal(path, base::timedPlannerTerminationCondition(maxTime), samplingAttempts, rangeRatio,
758  snapToVertex);
759 }
760 
762  unsigned int samplingAttempts, double rangeRatio,
763  double snapToVertex)
764 {
765  if (path.getStateCount() < 2)
766  return false;
767 
768  if (!gsr_)
769  {
770  OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
771  return false;
772  }
773 
774  unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
775  unsigned int failedTries = 0;
776  bool betterGoal = false;
777 
778  const base::StateSpacePtr &ss = si_->getStateSpace();
779  std::vector<base::State *> &states = path.getStates();
780 
781  // dists[i] contains the cumulative length of the path up to and including state i
782  std::vector<base::Cost> costs(states.size(), obj_->identityCost());
783  std::vector<double> dists(states.size(), 0.0);
784  for (unsigned int i = 1; i < dists.size(); ++i)
785  {
786  costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
787  dists[i] = dists[i - 1] + si_->distance(states[i - 1], states[i]);
788  if (dists[i] < 0)
789  {
790  OMPL_WARN("%s: Somehow computed negative distance!.", "PathSimplifier::findBetterGoal");
791  return false;
792  }
793  }
794 
795  // Sampled states closer than 'threshold' distance to any existing state in the path
796  // are snapped to the close state
797  double threshold = dists.back() * snapToVertex;
798  // The range (distance) of a single connection that will be attempted
799  double rd = rangeRatio * dists.back();
800 
801  base::State *temp = si_->allocState();
802  base::State *tempGoal = si_->allocState();
803 
804  while (!ptc && failedTries++ < maxGoals && !betterGoal)
805  {
806  gsr_->sampleGoal(tempGoal);
807 
808  // Goal state is not compatible with the start state
809  if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
810  continue;
811 
812  unsigned int numSamples = 0;
813  while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
814  {
815  // sample a state within rangeRatio
816  double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0),
817  dists.back()); // Sample a random point within rd of the end of the path
818 
819  auto end = std::lower_bound(dists.begin(), dists.end(), t);
820  auto start = end;
821  while (start != dists.begin() && *start >= t)
822  start -= 1;
823 
824  unsigned int startIndex = start - dists.begin();
825  unsigned int endIndex = end - dists.begin();
826 
827  // Snap the random point to the nearest vertex, if within the threshold
828  if (t - (*start) < threshold) // snap to the starting waypoint
829  endIndex = startIndex;
830  if ((*end) - t < threshold) // snap to the ending waypoint
831  startIndex = endIndex;
832 
833  // Compute the state value and the accumulated cost up to that state
834  base::Cost costToCome = costs[startIndex];
835  base::State *state;
836  if (startIndex == endIndex)
837  {
838  state = states[startIndex];
839  }
840  else
841  {
842  double tSeg = (t - (*start)) / (*end - *start);
843  ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
844  state = temp;
845 
846  costToCome = obj_->combineCosts(costToCome, obj_->motionCost(states[startIndex], state));
847  }
848 
849  base::Cost costToGo = obj_->motionCost(state, tempGoal);
850  base::Cost candidateCost = obj_->combineCosts(costToCome, costToGo);
851 
852  // Make sure we improve before attempting validation
853  if (obj_->isCostBetterThan(candidateCost, costs.back()) && si_->checkMotion(state, tempGoal))
854  {
855  // insert the new states
856  if (startIndex == endIndex)
857  {
858  // new intermediate state
859  si_->copyState(states[startIndex], state);
860  // new goal state
861  si_->copyState(states[startIndex + 1], tempGoal);
862 
863  if (freeStates_)
864  {
865  for (size_t i = startIndex + 2; i < states.size(); ++i)
866  si_->freeState(states[i]);
867  }
868  states.erase(states.begin() + startIndex + 2, states.end());
869  }
870  else
871  {
872  // overwriting the end of the segment with the new state
873  si_->copyState(states[endIndex], state);
874  if (endIndex == states.size() - 1)
875  {
876  path.append(tempGoal);
877  }
878  else
879  {
880  // adding goal new goal state
881  si_->copyState(states[endIndex + 1], tempGoal);
882  if (freeStates_)
883  {
884  for (size_t i = endIndex + 2; i < states.size(); ++i)
885  si_->freeState(states[i]);
886  }
887  states.erase(states.begin() + endIndex + 2, states.end());
888  }
889  }
890 
891  // fix the helper variables
892  costs.resize(states.size(), obj_->identityCost());
893  dists.resize(states.size(), 0.0);
894  for (unsigned int j = std::max(1u, startIndex); j < dists.size(); ++j)
895  {
896  costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
897  dists[j] = dists[j - 1] + si_->distance(states[j - 1], states[j]);
898  }
899 
900  betterGoal = true;
901  }
902  }
903  }
904 
905  si_->freeState(temp);
906  si_->freeState(tempGoal);
907 
908  return betterGoal;
909 }
910 
911 int ompl::geometric::PathSimplifier::selectAlongPath(std::vector<double> dists, std::vector<base::State *> states,
912  double distTo, double threshold, base::State *select_state,
913  int &pos)
914 {
915  if (distTo < 0)
916  distTo = 0;
917  else if (distTo > dists.back())
918  distTo = dists.back();
919 
920  int index = -1;
921  auto pit = std::lower_bound(dists.begin(), dists.end(), distTo);
922  pos = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
923 
924  if (pos == 0 || dists[pos] - distTo < threshold)
925  index = pos;
926  else
927  {
928  while (pos > 0 && distTo < dists[pos])
929  --pos;
930  if (distTo - dists[pos] < threshold)
931  index = pos;
932  }
933 
934  if (index >= 0)
935  {
936  si_->copyState(select_state, states[index]);
937  return index;
938  }
939  else
940  {
941  double t = (distTo - dists[pos]) / (dists[pos + 1] - dists[pos]);
942  si_->getStateSpace()->interpolate(states[pos], states[pos + 1], t, select_state);
943  return -1;
944  }
945 }
bool 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. Return false iff the simplified path is not valid.
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...
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
base::OptimizationObjectivePtr obj_
The optimization objective to use when making improvements. Will be used on all methods except reduce...
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
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...
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...
PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal=ompl::base::GoalPtr(), base::OptimizationObjectivePtr obj=nullptr)
Create an instance for a specified space information. Optionally, a GoalSampleableRegion may be passe...
base::SpaceInformationPtr si_
The space information this path simplifier uses.
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::OptimizationObjective.
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).
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
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...
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...