37 #include "ompl/geometric/PathSimplifier.h"
38 #include "ompl/tools/config/MagicConstants.h"
52 std::vector<base::State*> &states = path.
getStates();
57 for (
unsigned int s = 0 ; s < maxSteps ; ++s)
61 unsigned int i = 2, u = 0, n1 = states.size() - 1;
64 if (si->isValid(states[i - 1]))
66 si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
67 si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
68 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
69 if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
71 if (si->distance(states[i], temp1) > minChange)
73 si->copyState(states[i], temp1);
98 if (maxEmptySteps == 0)
102 unsigned int nochange = 0;
104 std::vector<base::State*> &states = path.
getStates();
106 if (si->checkMotion(states.front(), states.back()))
108 for (std::size_t i = 2 ; i < states.size() ; ++i)
109 si->freeState(states[i-1]);
110 std::vector<base::State*> newStates(2);
111 newStates[0] = states.front();
112 newStates[1] = states.back();
113 states.swap(newStates);
117 for (
unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
119 int count = states.size();
120 int maxN = count - 1;
121 int range = 1 + (int)(floor(0.5 + (
double)count * rangeRatio));
123 int p1 = rng_.uniformInt(0, maxN);
124 int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
125 if (abs(p1 - p2) < 2)
139 if (si->checkMotion(states[p1], states[p2]))
141 for (
int j = p1 + 1 ; j < p2 ; ++j)
142 si->freeState(states[j]);
143 states.erase(states.begin() + p1 + 1, states.begin() + p2);
159 if (maxEmptySteps == 0)
163 std::vector<base::State*> &states = path.
getStates();
165 std::vector<double> dists(states.size(), 0.0);
166 for (
unsigned int i = 1 ; i < dists.size() ; ++i)
167 dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
168 double threshold = dists.back() * snapToVertex;
169 double rd = rangeRatio * dists.back();
174 unsigned int nochange = 0;
175 for (
unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
180 double p0 = rng_.uniformReal(0.0, dists.back());
181 std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0);
182 int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
184 if (pos0 == 0 || dists[pos0] - p0 < threshold)
188 while (pos0 > 0 && p0 < dists[pos0])
190 if (p0 - dists[pos0] < threshold)
197 double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back()));
198 pit = std::lower_bound(dists.begin(), dists.end(), p1);
199 int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
201 if (pos1 == 0 || dists[pos1] - p1 < threshold)
205 while (pos1 > 0 && p1 < dists[pos1])
207 if (p1 - dists[pos1] < threshold)
211 if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
212 pos0 + 1 == index1 || pos1 + 1 == index0 ||
213 (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
220 t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
221 si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
229 t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
230 si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
234 if (si->checkMotion(s0, s1))
238 std::swap(pos0, pos1);
239 std::swap(index0, index1);
244 if (index0 < 0 && index1 < 0)
246 if (pos0 + 1 == pos1)
248 si->copyState(states[pos1], s0);
249 states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
253 for (
int j = pos0 + 2 ; j < pos1 ; ++j)
254 si->freeState(states[j]);
255 si->copyState(states[pos0 + 1], s0);
256 si->copyState(states[pos1], s1);
257 states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
261 if (index0 >= 0 && index1 >= 0)
263 for (
int j = index0 + 1 ; j < index1 ; ++j)
264 si->freeState(states[j]);
265 states.erase(states.begin() + index0 + 1, states.begin() + index1);
268 if (index0 < 0 && index1 >= 0)
270 for (
int j = pos0 + 2 ; j < index1 ; ++j)
271 si->freeState(states[j]);
272 si->copyState(states[pos0 + 1], s0);
273 states.erase(states.begin() + pos0 + 2, states.begin() + index1);
276 if (index0 >= 0 && index1 < 0)
278 for (
int j = index0 + 1 ; j < pos1 ; ++j)
279 si->freeState(states[j]);
280 si->copyState(states[pos1], s1);
281 states.erase(states.begin() + index0 + 1, states.begin() + pos1);
285 dists.resize(states.size(), 0.0);
286 for (
unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
287 dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
288 threshold = dists.back() * snapToVertex;
289 rd = rangeRatio * dists.back();
295 si->freeState(temp1);
296 si->freeState(temp0);
308 if (maxEmptySteps == 0)
312 std::vector<base::State*> &states = path.
getStates();
315 std::map<std::pair<const base::State*, const base::State*>,
double> distances;
316 for (
unsigned int i = 0 ; i < states.size() ; ++i)
317 for (
unsigned int j = i + 2 ; j < states.size() ; ++j)
318 distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
321 unsigned int nochange = 0;
322 for (
unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
325 double minDist = std::numeric_limits<double>::infinity();
328 for (
unsigned int i = 0 ; i < states.size() ; ++i)
329 for (
unsigned int j = i + 2 ; j < states.size() ; ++j)
331 double d = distances[std::make_pair(states[i], states[j])];
340 if (p1 >= 0 && p2 >= 0)
342 if (si->checkMotion(states[p1], states[p2]))
344 for (
int i = p1 + 1 ; i < p2 ; ++i)
345 si->freeState(states[i]);
346 states.erase(states.begin() + p1 + 1, states.begin() + p2);
351 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
361 reduceVertices(path);
362 collapseCloseVertices(path);
363 smoothBSpline(path, 4, path.
length()/100.0);
366 logWarn(
"Solution path may slightly touch on an invalid region of the state space");
369 logDebug(
"The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
383 bool tryMore =
false;
385 tryMore = reduceVertices(path);
389 collapseCloseVertices(path);
393 while (tryMore && ptc() ==
false && ++times <= 5)
394 tryMore = reduceVertices(path);
398 tryMore = shortcutPath(path);
404 while (tryMore && ptc() ==
false && ++times <= 5)
405 tryMore = shortcutPath(path);
409 smoothBSpline(path, 3, path.
length()/100.0);
414 logWarn(
"Solution path may slightly touch on an invalid region of the state space");
417 logDebug(
"The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");