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 
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 
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.emplace_back(si->distance(states[i], states[i + 1]), obj_->motionCost(states[i], states[i + 1]), i);
405 
406  // Sort so highest costs are first
407  std::sort(distCostIndices.begin(), distCostIndices.end(),
408  [this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
409  return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
410  });
411 
412  double threshold = dists.back() * snapToVertex;
413 
414  bool result = false;
415  unsigned int nochange = 0;
416 
417  base::StateSamplerPtr sampler = si->allocStateSampler();
418 
419  base::State *perturb_state = si->allocState();
420  base::State *before_state = si->allocState();
421  base::State *after_state = si->allocState();
422  base::State *new_state = si->allocState();
423 
424  // Attempt perturbing maxSteps times or when no improvement is found after
425  // maxEmptySteps attempts, whichever comes first.
426  for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; i++, nochange++)
427  {
428  // select a configuration on the path, biased towards high cost segments.
429  // Get a random real from 0 to the path length, biased towards 0.
430  double costBias = -1 * rng_.halfNormalReal(-dists.back(), 0.0, 20.0);
431  unsigned int z = 0;
432  if (costBias >= dists.back())
433  {
434  z = distCostIndices.size() - 1;
435  costBias = std::get<0>(distCostIndices[z]);
436  }
437  else
438  {
439  // Using our sorted cost vector, find the segment we want to use.
440  while (costBias - std::get<0>(distCostIndices[z]) > std::numeric_limits<double>::epsilon())
441  {
442  costBias -= std::get<0>(distCostIndices[z]);
443  z++;
444  }
445  }
446 
447  int pos, pos_before, pos_after;
448  double distTo = dists[std::get<2>(distCostIndices[z])] + costBias;
449  selectAlongPath(dists, states, distTo, threshold, perturb_state, pos);
450 
451  // Get before state and after state, that are around stepsize/2 on either side of perturb state.
452  int index_before = selectAlongPath(dists, states, distTo - stepSize / 2.0, threshold, before_state, pos_before);
453  int index_after = selectAlongPath(dists, states, distTo + stepSize / 2.0, threshold, after_state, pos_after);
454 
455  if (index_before >= 0 && index_after >= 0 && index_before == index_after)
456  {
457  continue;
458  }
459 
460  // Pick a random direction to extend and take a stepSize step in that direction.
461  sampler->sampleUniform(new_state);
462  double dist = si->distance(perturb_state, new_state);
463  si->getStateSpace()->interpolate(perturb_state, new_state, stepSize / dist, new_state);
464 
465  // Check for validity of the new path to the new state.
466  if (si->checkMotion(before_state, new_state) && si->checkMotion(new_state, after_state))
467  {
468  // Now check for improved cost. Get the original cost along the path.
469  base::Cost alongPath;
470  if (pos_before == pos_after)
471  {
472  alongPath = obj_->motionCost(before_state, after_state);
473  }
474  else
475  {
476  // The partial cost from before_state to the first waypoint.
477  alongPath =
478  (index_before >= 0) ? obj_->identityCost() : obj_->motionCost(before_state, states[pos_before + 1]);
479  int posTemp = (index_before >= 0) ? index_before : pos_before + 1;
480  while (posTemp < pos_after)
481  {
482  alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
483  posTemp++;
484  }
485  base::Cost afterPartialCost =
486  (index_after >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos_after], after_state);
487  alongPath = obj_->combineCosts(alongPath, afterPartialCost);
488  }
489 
490  base::Cost newCost =
491  obj_->combineCosts(obj_->motionCost(before_state, new_state), obj_->motionCost(new_state, after_state));
492  if (obj_->isCostBetterThan(alongPath, newCost) || obj_->isCostEquivalentTo(alongPath, newCost))
493  {
494  // Cost along the current path is better than the perturbed path.
495  continue;
496  }
497 
498  // Modify the path with the new state.
499  if (index_before < 0 && index_after < 0)
500  {
501  if (pos_before == pos_after)
502  {
503  // Insert all 3 states; new_state goes before after_state, and before_state
504  // goes before new_state.
505  states.insert(states.begin() + pos_before + 1, si->cloneState(after_state));
506  states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
507  states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
508  }
509  else if (pos_before + 1 == pos_after)
510  {
511  si->copyState(states[pos_after], before_state);
512  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
513  states.insert(states.begin() + pos_after + 1, si->cloneState(new_state));
514  }
515  else if (pos_before + 2 == pos_after)
516  {
517  si->copyState(states[pos_before + 1], before_state);
518  si->copyState(states[pos_after], new_state);
519  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
520  }
521  else
522  {
523  si->copyState(states[pos_before + 1], before_state);
524  si->copyState(states[pos_before + 2], new_state);
525  si->copyState(states[pos_before + 3], after_state);
526  if (freeStates_)
527  for (int j = pos_before + 4; j < pos_after + 1; ++j)
528  si->freeState(states[j]);
529  states.erase(states.begin() + pos_before + 4, states.begin() + pos_after + 1);
530  }
531  }
532  else if (index_before >= 0 && index_after >= 0)
533  {
534  states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
535  if (freeStates_)
536  for (int j = index_before + 2; j < index_after + 1; ++j)
537  si->freeState(states[j]);
538  states.erase(states.begin() + index_before + 2, states.begin() + index_after + 1);
539  }
540  else if (index_before < 0 && index_after >= 0)
541  {
542  if (index_after > pos_before + 1)
543  {
544  si->copyState(states[pos_before + 1], before_state);
545  states.insert(states.begin() + pos_before + 2, si->cloneState(new_state));
546  if (freeStates_)
547  for (int j = pos_before + 3; j < index_after + 1; ++j)
548  si->freeState(states[j]);
549  states.erase(states.begin() + pos_before + 3, states.begin() + index_after + 1);
550  }
551  else
552  {
553  states.insert(states.begin() + pos_before + 1, si->cloneState(new_state));
554  states.insert(states.begin() + pos_before + 1, si->cloneState(before_state));
555  }
556  }
557  else if (index_before >= 0 && index_after < 0)
558  {
559  if (pos_after > index_before)
560  {
561  si->copyState(states[pos_after], new_state);
562  states.insert(states.begin() + pos_after + 1, si->cloneState(after_state));
563  if (freeStates_)
564  for (int j = index_before + 1; j < pos_after; ++j)
565  si->freeState(states[j]);
566  states.erase(states.begin() + index_before + 1, states.begin() + pos_after);
567  }
568  else
569  {
570  states.insert(states.begin() + index_before + 1, si->cloneState(after_state));
571  states.insert(states.begin() + index_before + 1, si->cloneState(new_state));
572  }
573  }
574 
575  // fix the helper variables
576  dists.resize(states.size(), 0.0);
577  for (unsigned int j = pos_before + 1; j < dists.size(); ++j)
578  {
579  dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
580  }
581  distCostIndices.clear();
582  for (unsigned int i = 0; i < states.size() - 1; i++)
583  distCostIndices.emplace_back(si->distance(states[i], states[i + 1]),
584  obj_->motionCost(states[i], states[i + 1]), i);
585 
586  // Sort so highest costs are first
587  std::sort(
588  distCostIndices.begin(), distCostIndices.end(),
589  [this](std::tuple<double, base::Cost, unsigned int> a, std::tuple<double, base::Cost, unsigned int> b) {
590  return obj_->isCostBetterThan(std::get<1>(b), std::get<1>(a));
591  });
592  threshold = dists.back() * snapToVertex;
593  result = true;
594  nochange = 0;
595  }
596  }
597  si->freeState(perturb_state);
598  si->freeState(before_state);
599  si->freeState(after_state);
600  si->freeState(new_state);
601 
602  return result;
603 }
604 
606  unsigned int maxEmptySteps)
607 {
608  if (path.getStateCount() < 3)
609  return false;
610 
611  if (maxSteps == 0)
612  maxSteps = path.getStateCount();
613 
614  if (maxEmptySteps == 0)
615  maxEmptySteps = path.getStateCount();
616 
617  const base::SpaceInformationPtr &si = path.getSpaceInformation();
618  std::vector<base::State *> &states = path.getStates();
619 
620  // compute pair-wise distances in path (construct only half the matrix)
621  std::map<std::pair<const base::State *, const base::State *>, double> distances;
622  for (unsigned int i = 0; i < states.size(); ++i)
623  for (unsigned int j = i + 2; j < states.size(); ++j)
624  distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
625 
626  bool result = false;
627  unsigned int nochange = 0;
628  for (unsigned int s = 0; s < maxSteps && nochange < maxEmptySteps; ++s, ++nochange)
629  {
630  // find closest pair of points
631  double minDist = std::numeric_limits<double>::infinity();
632  int p1 = -1;
633  int p2 = -1;
634  for (unsigned int i = 0; i < states.size(); ++i)
635  for (unsigned int j = i + 2; j < states.size(); ++j)
636  {
637  double d = distances[std::make_pair(states[i], states[j])];
638  if (d < minDist)
639  {
640  minDist = d;
641  p1 = i;
642  p2 = j;
643  }
644  }
645 
646  if (p1 >= 0 && p2 >= 0)
647  {
648  if (si->checkMotion(states[p1], states[p2]))
649  {
650  if (freeStates_)
651  for (int i = p1 + 1; i < p2; ++i)
652  si->freeState(states[i]);
653  states.erase(states.begin() + p1 + 1, states.begin() + p2);
654  result = true;
655  nochange = 0;
656  }
657  else
658  distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
659  }
660  else
661  break;
662  }
663  return result;
664 }
665 
667 {
669  return simplify(path, neverTerminate);
670 }
671 
672 bool ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime, bool atLeastOnce)
673 {
674  return simplify(path, base::timedPlannerTerminationCondition(maxTime), atLeastOnce);
675 }
676 
678  bool atLeastOnce)
679 {
680  if (path.getStateCount() < 3)
681  return true;
682 
683  bool tryMore = true, valid = true;
684  while ((ptc == false || atLeastOnce) && tryMore)
685  {
686  // if the space is metric, we can do some additional smoothing
687  if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
688  {
689  bool metricTryMore = true;
690  unsigned int times = 0;
691  do
692  {
693  bool shortcut = shortcutPath(path); // split path segments, not just vertices
694  bool better_goal =
695  gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
696 
697  metricTryMore = shortcut || better_goal;
698  } while ((ptc == false || atLeastOnce) && ++times <= 5 && metricTryMore);
699 
700  // smooth the path with BSpline interpolation
701  if (ptc == false || atLeastOnce)
702  smoothBSpline(path, 3, path.length() / 100.0);
703 
704  if (ptc == false || atLeastOnce)
705  {
706  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
707  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
708  if (!p.second)
709  {
710  valid = false;
711  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
712  }
713  else if (!p.first)
714  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but "
715  "it was "
716  "successfully fixed.");
717  }
718  }
719 
720  // try a randomized step of connecting vertices
721  if (ptc == false || atLeastOnce)
722  tryMore = reduceVertices(path);
723 
724  // try to collapse close-by vertices
725  if (ptc == false || atLeastOnce)
726  collapseCloseVertices(path);
727 
728  // try to reduce verices some more, if there is any point in doing so
729  unsigned int times = 0;
730  while ((ptc == false || atLeastOnce) && tryMore && ++times <= 5)
731  tryMore = reduceVertices(path);
732 
733  if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
734  {
735  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
736  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
737  if (!p.second)
738  {
739  valid = false;
740  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
741  }
742  else if (!p.first)
743  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it "
744  "was "
745  "successfully fixed.");
746  }
747 
748  atLeastOnce = false;
749  }
750  return valid || path.check();
751 }
752 
753 bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts,
754  double rangeRatio, double snapToVertex)
755 {
756  return findBetterGoal(path, base::timedPlannerTerminationCondition(maxTime), samplingAttempts, rangeRatio,
757  snapToVertex);
758 }
759 
761  unsigned int samplingAttempts, double rangeRatio,
762  double snapToVertex)
763 {
764  if (path.getStateCount() < 2)
765  return false;
766 
767  if (!gsr_)
768  {
769  OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
770  return false;
771  }
772 
773  unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
774  unsigned int failedTries = 0;
775  bool betterGoal = false;
776 
777  const base::StateSpacePtr &ss = si_->getStateSpace();
778  std::vector<base::State *> &states = path.getStates();
779 
780  // dists[i] contains the cumulative length of the path up to and including state i
781  std::vector<base::Cost> costs(states.size(), obj_->identityCost());
782  std::vector<double> dists(states.size(), 0.0);
783  for (unsigned int i = 1; i < dists.size(); ++i)
784  {
785  costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
786  dists[i] = dists[i - 1] + si_->distance(states[i - 1], states[i]);
787  if (dists[i] < 0)
788  {
789  OMPL_WARN("%s: Somehow computed negative distance!.", "PathSimplifier::findBetterGoal");
790  return false;
791  }
792  }
793 
794  // Sampled states closer than 'threshold' distance to any existing state in the path
795  // are snapped to the close state
796  double threshold = dists.back() * snapToVertex;
797  // The range (distance) of a single connection that will be attempted
798  double rd = rangeRatio * dists.back();
799 
800  base::State *temp = si_->allocState();
801  base::State *tempGoal = si_->allocState();
802 
803  while (!ptc && failedTries++ < maxGoals && !betterGoal)
804  {
805  gsr_->sampleGoal(tempGoal);
806 
807  // Goal state is not compatible with the start state
808  if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
809  continue;
810 
811  unsigned int numSamples = 0;
812  while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
813  {
814  // sample a state within rangeRatio
815  double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0),
816  dists.back()); // Sample a random point within rd of the end of the path
817 
818  auto end = std::lower_bound(dists.begin(), dists.end(), t);
819  auto start = end;
820  while (start != dists.begin() && *start >= t)
821  start -= 1;
822 
823  unsigned int startIndex = start - dists.begin();
824  unsigned int endIndex = end - dists.begin();
825 
826  // Snap the random point to the nearest vertex, if within the threshold
827  if (t - (*start) < threshold) // snap to the starting waypoint
828  endIndex = startIndex;
829  if ((*end) - t < threshold) // snap to the ending waypoint
830  startIndex = endIndex;
831 
832  // Compute the state value and the accumulated cost up to that state
833  base::Cost costToCome = costs[startIndex];
834  base::State *state;
835  if (startIndex == endIndex)
836  {
837  state = states[startIndex];
838  }
839  else
840  {
841  double tSeg = (t - (*start)) / (*end - *start);
842  ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
843  state = temp;
844 
845  costToCome = obj_->combineCosts(costToCome, obj_->motionCost(states[startIndex], state));
846  }
847 
848  base::Cost costToGo = obj_->motionCost(state, tempGoal);
849  base::Cost candidateCost = obj_->combineCosts(costToCome, costToGo);
850 
851  // Make sure we improve before attempting validation
852  if (obj_->isCostBetterThan(candidateCost, costs.back()) && si_->checkMotion(state, tempGoal))
853  {
854  // insert the new states
855  if (startIndex == endIndex)
856  {
857  // new intermediate state
858  si_->copyState(states[startIndex], state);
859  // new goal state
860  si_->copyState(states[startIndex + 1], tempGoal);
861 
862  if (freeStates_)
863  {
864  for (size_t i = startIndex + 2; i < states.size(); ++i)
865  si_->freeState(states[i]);
866  }
867  states.erase(states.begin() + startIndex + 2, states.end());
868  }
869  else
870  {
871  // overwriting the end of the segment with the new state
872  si_->copyState(states[endIndex], state);
873  if (endIndex == states.size() - 1)
874  {
875  path.append(tempGoal);
876  }
877  else
878  {
879  // adding goal new goal state
880  si_->copyState(states[endIndex + 1], tempGoal);
881  if (freeStates_)
882  {
883  for (size_t i = endIndex + 2; i < states.size(); ++i)
884  si_->freeState(states[i]);
885  }
886  states.erase(states.begin() + endIndex + 2, states.end());
887  }
888  }
889 
890  // fix the helper variables
891  costs.resize(states.size(), obj_->identityCost());
892  dists.resize(states.size(), 0.0);
893  for (unsigned int j = std::max(1u, startIndex); j < dists.size(); ++j)
894  {
895  costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
896  dists[j] = dists[j - 1] + si_->distance(states[j - 1], states[j]);
897  }
898 
899  betterGoal = true;
900  }
901  }
902  }
903 
904  si_->freeState(temp);
905  si_->freeState(tempGoal);
906 
907  return betterGoal;
908 }
909 
910 int ompl::geometric::PathSimplifier::selectAlongPath(std::vector<double> dists, std::vector<base::State *> states,
911  double distTo, double threshold, base::State *select_state,
912  int &pos)
913 {
914  if (distTo < 0)
915  distTo = 0;
916  else if (distTo > dists.back())
917  distTo = dists.back();
918 
919  int index = -1;
920  auto pit = std::lower_bound(dists.begin(), dists.end(), distTo);
921  pos = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
922 
923  if (pos == 0 || dists[pos] - distTo < threshold)
924  index = pos;
925  else
926  {
927  while (pos > 0 && distTo < dists[pos])
928  --pos;
929  if (distTo - dists[pos] < threshold)
930  index = pos;
931  }
932 
933  if (index >= 0)
934  {
935  si_->copyState(select_state, states[index]);
936  return index;
937  }
938  else
939  {
940  double t = (distTo - dists[pos]) / (dists[pos + 1] - dists[pos]);
941  si_->getStateSpace()->interpolate(states[pos], states[pos + 1], t, select_state);
942  return -1;
943  }
944 }
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 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...
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...
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
Definition of an abstract state.
Definition: State.h:50
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...
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:48
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...
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 of a geometric path.
Definition: PathGeometric.h:66
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
bool simplifyMax(PathGeometric &path)
Given a path, attempt to remove vertices from it while keeping the path valid. Then,...
base::SpaceInformationPtr si_
The space information this path simplifier uses.
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...
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met.
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70