All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 */
36 
37 #include "ompl/geometric/PathSimplifier.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include <algorithm>
40 #include <limits>
41 #include <cstdlib>
42 #include <cmath>
43 #include <map>
44 
46 {
47  return freeStates_;
48 }
49 
51 {
52  freeStates_ = flag;
53 }
54 
55 /* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
56 void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
57 {
58  if (path.getStateCount() < 3)
59  return;
60 
62  std::vector<base::State*> &states = path.getStates();
63 
64  base::State *temp1 = si->allocState();
65  base::State *temp2 = si->allocState();
66 
67  for (unsigned int s = 0 ; s < maxSteps ; ++s)
68  {
69  path.subdivide();
70 
71  unsigned int i = 2, u = 0, n1 = states.size() - 1;
72  while (i < n1)
73  {
74  if (si->isValid(states[i - 1]))
75  {
76  si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
77  si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
78  si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
79  if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
80  {
81  if (si->distance(states[i], temp1) > minChange)
82  {
83  si->copyState(states[i], temp1);
84  ++u;
85  }
86  }
87  }
88 
89  i += 2;
90  }
91 
92  if (u == 0)
93  break;
94  }
95 
96  si->freeState(temp1);
97  si->freeState(temp2);
98 }
99 
100 bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
101 {
102  if (path.getStateCount() < 3)
103  return false;
104 
105  if (maxSteps == 0)
106  maxSteps = path.getStateCount();
107 
108  if (maxEmptySteps == 0)
109  maxEmptySteps = path.getStateCount();
110 
111  bool result = false;
112  unsigned int nochange = 0;
114  std::vector<base::State*> &states = path.getStates();
115 
116  if (si->checkMotion(states.front(), states.back()))
117  {
118  if (freeStates_)
119  for (std::size_t i = 2 ; i < states.size() ; ++i)
120  si->freeState(states[i-1]);
121  std::vector<base::State*> newStates(2);
122  newStates[0] = states.front();
123  newStates[1] = states.back();
124  states.swap(newStates);
125  result = true;
126  }
127  else
128  for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
129  {
130  int count = states.size();
131  int maxN = count - 1;
132  int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
133 
134  int p1 = rng_.uniformInt(0, maxN);
135  int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
136  if (abs(p1 - p2) < 2)
137  {
138  if (p1 < maxN - 1)
139  p2 = p1 + 2;
140  else
141  if (p1 > 1)
142  p2 = p1 - 2;
143  else
144  continue;
145  }
146 
147  if (p1 > p2)
148  std::swap(p1, p2);
149 
150  if (si->checkMotion(states[p1], states[p2]))
151  {
152  if (freeStates_)
153  for (int j = p1 + 1 ; j < p2 ; ++j)
154  si->freeState(states[j]);
155  states.erase(states.begin() + p1 + 1, states.begin() + p2);
156  nochange = 0;
157  result = true;
158  }
159  }
160  return result;
161 }
162 
163 bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
164 {
165  if (path.getStateCount() < 3)
166  return false;
167 
168  if (maxSteps == 0)
169  maxSteps = path.getStateCount();
170 
171  if (maxEmptySteps == 0)
172  maxEmptySteps = path.getStateCount();
173 
175  std::vector<base::State*> &states = path.getStates();
176 
177  std::vector<double> dists(states.size(), 0.0);
178  for (unsigned int i = 1 ; i < dists.size() ; ++i)
179  dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
180  double threshold = dists.back() * snapToVertex;
181  double rd = rangeRatio * dists.back();
182 
183  base::State *temp0 = si->allocState();
184  base::State *temp1 = si->allocState();
185  bool result = false;
186  unsigned int nochange = 0;
187  for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
188  {
189  base::State *s0 = NULL;
190  int index0 = -1;
191  double t0 = 0.0;
192  double p0 = rng_.uniformReal(0.0, dists.back());
193  std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0);
194  int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
195 
196  if (pos0 == 0 || dists[pos0] - p0 < threshold)
197  index0 = pos0;
198  else
199  {
200  while (pos0 > 0 && p0 < dists[pos0])
201  --pos0;
202  if (p0 - dists[pos0] < threshold)
203  index0 = pos0;
204  }
205 
206  base::State *s1 = NULL;
207  int index1 = -1;
208  double t1 = 0.0;
209  double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back()));
210  pit = std::lower_bound(dists.begin(), dists.end(), p1);
211  int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
212 
213  if (pos1 == 0 || dists[pos1] - p1 < threshold)
214  index1 = pos1;
215  else
216  {
217  while (pos1 > 0 && p1 < dists[pos1])
218  --pos1;
219  if (p1 - dists[pos1] < threshold)
220  index1 = pos1;
221  }
222 
223  if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
224  pos0 + 1 == index1 || pos1 + 1 == index0 ||
225  (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
226  continue;
227 
228  if (index0 >= 0)
229  s0 = states[index0];
230  else
231  {
232  t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
233  si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
234  s0 = temp0;
235  }
236 
237  if (index1 >= 0)
238  s1 = states[index1];
239  else
240  {
241  t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
242  si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
243  s1 = temp1;
244  }
245 
246  if (si->checkMotion(s0, s1))
247  {
248  if (pos0 > pos1)
249  {
250  std::swap(pos0, pos1);
251  std::swap(index0, index1);
252  std::swap(s0, s1);
253  std::swap(t0, t1);
254  }
255 
256  if (index0 < 0 && index1 < 0)
257  {
258  if (pos0 + 1 == pos1)
259  {
260  si->copyState(states[pos1], s0);
261  states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
262  }
263  else
264  {
265  if (freeStates_)
266  for (int j = pos0 + 2 ; j < pos1 ; ++j)
267  si->freeState(states[j]);
268  si->copyState(states[pos0 + 1], s0);
269  si->copyState(states[pos1], s1);
270  states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
271  }
272  }
273  else
274  if (index0 >= 0 && index1 >= 0)
275  {
276  if (freeStates_)
277  for (int j = index0 + 1 ; j < index1 ; ++j)
278  si->freeState(states[j]);
279  states.erase(states.begin() + index0 + 1, states.begin() + index1);
280  }
281  else
282  if (index0 < 0 && index1 >= 0)
283  {
284  if (freeStates_)
285  for (int j = pos0 + 2 ; j < index1 ; ++j)
286  si->freeState(states[j]);
287  si->copyState(states[pos0 + 1], s0);
288  states.erase(states.begin() + pos0 + 2, states.begin() + index1);
289  }
290  else
291  if (index0 >= 0 && index1 < 0)
292  {
293  if (freeStates_)
294  for (int j = index0 + 1 ; j < pos1 ; ++j)
295  si->freeState(states[j]);
296  si->copyState(states[pos1], s1);
297  states.erase(states.begin() + index0 + 1, states.begin() + pos1);
298  }
299 
300  // fix the helper variables
301  dists.resize(states.size(), 0.0);
302  for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
303  dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
304  threshold = dists.back() * snapToVertex;
305  rd = rangeRatio * dists.back();
306  result = true;
307  nochange = 0;
308  }
309  }
310 
311  si->freeState(temp1);
312  si->freeState(temp0);
313  return result;
314 }
315 
316 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
317 {
318  if (path.getStateCount() < 3)
319  return false;
320 
321  if (maxSteps == 0)
322  maxSteps = path.getStateCount();
323 
324  if (maxEmptySteps == 0)
325  maxEmptySteps = path.getStateCount();
326 
328  std::vector<base::State*> &states = path.getStates();
329 
330  // compute pair-wise distances in path (construct only half the matrix)
331  std::map<std::pair<const base::State*, const base::State*>, double> distances;
332  for (unsigned int i = 0 ; i < states.size() ; ++i)
333  for (unsigned int j = i + 2 ; j < states.size() ; ++j)
334  distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
335 
336  bool result = false;
337  unsigned int nochange = 0;
338  for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
339  {
340  // find closest pair of points
341  double minDist = std::numeric_limits<double>::infinity();
342  int p1 = -1;
343  int p2 = -1;
344  for (unsigned int i = 0 ; i < states.size() ; ++i)
345  for (unsigned int j = i + 2 ; j < states.size() ; ++j)
346  {
347  double d = distances[std::make_pair(states[i], states[j])];
348  if (d < minDist)
349  {
350  minDist = d;
351  p1 = i;
352  p2 = j;
353  }
354  }
355 
356  if (p1 >= 0 && p2 >= 0)
357  {
358  if (si->checkMotion(states[p1], states[p2]))
359  {
360  if (freeStates_)
361  for (int i = p1 + 1 ; i < p2 ; ++i)
362  si->freeState(states[i]);
363  states.erase(states.begin() + p1 + 1, states.begin() + p2);
364  result = true;
365  nochange = 0;
366  }
367  else
368  distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
369  }
370  else
371  break;
372  }
373  return result;
374 }
375 
377 {
378  reduceVertices(path);
379  collapseCloseVertices(path);
380  // BSpline and checkAndRepair code only makes sense in a metric space.
381  if (si_->getStateSpace()->isMetricSpace())
382  {
383  smoothBSpline(path, 4, path.length()/100.0);
384  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
385  if (!p.second)
386  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
387  else
388  if (!p.first)
389  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
390  }
391 }
392 
394 {
395  simplify(path, base::timedPlannerTerminationCondition(maxTime));
396 }
397 
399 {
400  if (path.getStateCount() < 3)
401  return;
402 
403  // try a randomized step of connecting vertices
404  bool tryMore = false;
405  if (ptc == false)
406  tryMore = reduceVertices(path);
407 
408  // try to collapse close-by vertices
409  if (ptc == false)
410  collapseCloseVertices(path);
411 
412  // try to reduce verices some more, if there is any point in doing so
413  int times = 0;
414  while (tryMore && ptc == false && ++times <= 5)
415  tryMore = reduceVertices(path);
416 
417  // if the space is metric, we can do some additional smoothing
418  if(si_->getStateSpace()->isMetricSpace())
419  {
420  // run a more complex short-cut algorithm : allow splitting path segments
421  if (ptc == false)
422  tryMore = shortcutPath(path);
423  else
424  tryMore = false;
425 
426  // run the short-cut algorithm some more, if it makes a difference
427  times = 0;
428  while (tryMore && ptc == false && ++times <= 5)
429  tryMore = shortcutPath(path);
430 
431  // smooth the path with BSpline interpolation
432  if(ptc == false)
433  smoothBSpline(path, 3, path.length()/100.0);
434 
435  // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
436  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
437  if (!p.second)
438  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
439  else
440  if (!p.first)
441  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
442  }
443 }
bool shortcutPath(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33, double snapToVertex=0.005)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
void simplify(PathGeometric &path, double maxTime)
Run simplification algorithms on the path for at most maxTime seconds.
void 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.
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information associated to this class.
Definition: Path.h:82
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
std::pair< bool, bool > checkAndRepair(unsigned int attempts)
Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invali...
void subdivide(void)
Add a state at the middle of each segment.
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
bool reduceVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
#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
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification...
Definition of a geometric path.
Definition: PathGeometric.h:55
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...
bool freeStates_
Flag indicating whether the states removed from a motion should be freed.
virtual double length(void) const
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
void smoothBSpline(PathGeometric &path, unsigned int maxSteps=5, double minChange=std::numeric_limits< double >::epsilon())
Given a path, attempt to smooth it (the validity of the path is maintained).
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...