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 
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::shortcutPath(PathGeometric &path, unsigned int maxSteps,
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 
605 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps,
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 
666 bool ompl::geometric::PathSimplifier::simplifyMax(PathGeometric &path)
667 {
668  ompl::base::PlannerTerminationCondition neverTerminate = base::plannerNonTerminatingCondition();
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 
677 bool ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
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 
760 bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
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 }
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:325
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:321
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:605
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:386
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:672
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:753
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:666
ompl::geometric::PathSimplifier::si_
base::SpaceInformationPtr si_
The space information this path simplifier uses.
Definition: PathSimplifier.h:318
ompl::geometric::PathSimplifier::shortcutPath
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...
Definition: PathSimplifier.cpp:187
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