00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/geometric/PathSimplifier.h"
00038 #include "ompl/tools/config/MagicConstants.h"
00039 #include <algorithm>
00040 #include <limits>
00041 #include <cstdlib>
00042 #include <cmath>
00043 #include <map>
00044
00045
00046 void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
00047 {
00048 if (path.getStateCount() < 3)
00049 return;
00050
00051 const base::SpaceInformationPtr &si = path.getSpaceInformation();
00052 std::vector<base::State*> &states = path.getStates();
00053
00054 base::State *temp1 = si->allocState();
00055 base::State *temp2 = si->allocState();
00056
00057 for (unsigned int s = 0 ; s < maxSteps ; ++s)
00058 {
00059 path.subdivide();
00060
00061 unsigned int i = 2, u = 0, n1 = states.size() - 1;
00062 while (i < n1)
00063 {
00064 if (si->isValid(states[i - 1]))
00065 {
00066 si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
00067 si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
00068 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
00069 if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
00070 {
00071 if (si->distance(states[i], temp1) > minChange)
00072 {
00073 si->copyState(states[i], temp1);
00074 ++u;
00075 }
00076 }
00077 }
00078
00079 i += 2;
00080 }
00081
00082 if (u == 0)
00083 break;
00084 }
00085
00086 si->freeState(temp1);
00087 si->freeState(temp2);
00088 }
00089
00090 bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
00091 {
00092 if (path.getStateCount() < 3)
00093 return false;
00094
00095 if (maxSteps == 0)
00096 maxSteps = path.getStateCount();
00097
00098 if (maxEmptySteps == 0)
00099 maxEmptySteps = path.getStateCount();
00100
00101 bool result = false;
00102 unsigned int nochange = 0;
00103 const base::SpaceInformationPtr &si = path.getSpaceInformation();
00104 std::vector<base::State*> &states = path.getStates();
00105
00106 if (si->checkMotion(states.front(), states.back()))
00107 {
00108 for (std::size_t i = 2 ; i < states.size() ; ++i)
00109 si->freeState(states[i-1]);
00110 std::vector<base::State*> newStates(2);
00111 newStates[0] = states.front();
00112 newStates[1] = states.back();
00113 states.swap(newStates);
00114 result = true;
00115 }
00116 else
00117 for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00118 {
00119 int count = states.size();
00120 int maxN = count - 1;
00121 int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
00122
00123 int p1 = rng_.uniformInt(0, maxN);
00124 int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
00125 if (abs(p1 - p2) < 2)
00126 {
00127 if (p1 < maxN - 1)
00128 p2 = p1 + 2;
00129 else
00130 if (p1 > 1)
00131 p2 = p1 - 2;
00132 else
00133 continue;
00134 }
00135
00136 if (p1 > p2)
00137 std::swap(p1, p2);
00138
00139 if (si->checkMotion(states[p1], states[p2]))
00140 {
00141 for (int j = p1 + 1 ; j < p2 ; ++j)
00142 si->freeState(states[j]);
00143 states.erase(states.begin() + p1 + 1, states.begin() + p2);
00144 nochange = 0;
00145 result = true;
00146 }
00147 }
00148 return result;
00149 }
00150
00151 bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
00152 {
00153 if (path.getStateCount() < 3)
00154 return false;
00155
00156 if (maxSteps == 0)
00157 maxSteps = path.getStateCount();
00158
00159 if (maxEmptySteps == 0)
00160 maxEmptySteps = path.getStateCount();
00161
00162 const base::SpaceInformationPtr &si = path.getSpaceInformation();
00163 std::vector<base::State*> &states = path.getStates();
00164
00165 std::vector<double> dists(states.size(), 0.0);
00166 for (unsigned int i = 1 ; i < dists.size() ; ++i)
00167 dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
00168 double threshold = dists.back() * snapToVertex;
00169 double rd = rangeRatio * dists.back();
00170
00171 base::State *temp0 = si->allocState();
00172 base::State *temp1 = si->allocState();
00173 bool result = false;
00174 unsigned int nochange = 0;
00175 for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00176 {
00177 base::State *s0 = NULL;
00178 int index0 = -1;
00179 double t0 = 0.0;
00180 double p0 = rng_.uniformReal(0.0, dists.back());
00181 std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0);
00182 int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
00183
00184 if (pos0 == 0 || dists[pos0] - p0 < threshold)
00185 index0 = pos0;
00186 else
00187 {
00188 while (pos0 > 0 && p0 < dists[pos0])
00189 --pos0;
00190 if (p0 - dists[pos0] < threshold)
00191 index0 = pos0;
00192 }
00193
00194 base::State *s1 = NULL;
00195 int index1 = -1;
00196 double t1 = 0.0;
00197 double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back()));
00198 pit = std::lower_bound(dists.begin(), dists.end(), p1);
00199 int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
00200
00201 if (pos1 == 0 || dists[pos1] - p1 < threshold)
00202 index1 = pos1;
00203 else
00204 {
00205 while (pos1 > 0 && p1 < dists[pos1])
00206 --pos1;
00207 if (p1 - dists[pos1] < threshold)
00208 index1 = pos1;
00209 }
00210
00211 if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
00212 pos0 + 1 == index1 || pos1 + 1 == index0 ||
00213 (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
00214 continue;
00215
00216 if (index0 >= 0)
00217 s0 = states[index0];
00218 else
00219 {
00220 t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
00221 si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
00222 s0 = temp0;
00223 }
00224
00225 if (index1 >= 0)
00226 s1 = states[index1];
00227 else
00228 {
00229 t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
00230 si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
00231 s1 = temp1;
00232 }
00233
00234 if (si->checkMotion(s0, s1))
00235 {
00236 if (pos0 > pos1)
00237 {
00238 std::swap(pos0, pos1);
00239 std::swap(index0, index1);
00240 std::swap(s0, s1);
00241 std::swap(t0, t1);
00242 }
00243
00244 if (index0 < 0 && index1 < 0)
00245 {
00246 if (pos0 + 1 == pos1)
00247 {
00248 si->copyState(states[pos1], s0);
00249 states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
00250 }
00251 else
00252 {
00253 for (int j = pos0 + 2 ; j < pos1 ; ++j)
00254 si->freeState(states[j]);
00255 si->copyState(states[pos0 + 1], s0);
00256 si->copyState(states[pos1], s1);
00257 states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
00258 }
00259 }
00260 else
00261 if (index0 >= 0 && index1 >= 0)
00262 {
00263 for (int j = index0 + 1 ; j < index1 ; ++j)
00264 si->freeState(states[j]);
00265 states.erase(states.begin() + index0 + 1, states.begin() + index1);
00266 }
00267 else
00268 if (index0 < 0 && index1 >= 0)
00269 {
00270 for (int j = pos0 + 2 ; j < index1 ; ++j)
00271 si->freeState(states[j]);
00272 si->copyState(states[pos0 + 1], s0);
00273 states.erase(states.begin() + pos0 + 2, states.begin() + index1);
00274 }
00275 else
00276 if (index0 >= 0 && index1 < 0)
00277 {
00278 for (int j = index0 + 1 ; j < pos1 ; ++j)
00279 si->freeState(states[j]);
00280 si->copyState(states[pos1], s1);
00281 states.erase(states.begin() + index0 + 1, states.begin() + pos1);
00282 }
00283
00284
00285 dists.resize(states.size(), 0.0);
00286 for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
00287 dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
00288 threshold = dists.back() * snapToVertex;
00289 rd = rangeRatio * dists.back();
00290 result = true;
00291 nochange = 0;
00292 }
00293 }
00294
00295 si->freeState(temp1);
00296 si->freeState(temp0);
00297 return result;
00298 }
00299
00300 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
00301 {
00302 if (path.getStateCount() < 3)
00303 return false;
00304
00305 if (maxSteps == 0)
00306 maxSteps = path.getStateCount();
00307
00308 if (maxEmptySteps == 0)
00309 maxEmptySteps = path.getStateCount();
00310
00311 const base::SpaceInformationPtr &si = path.getSpaceInformation();
00312 std::vector<base::State*> &states = path.getStates();
00313
00314
00315 std::map<std::pair<const base::State*, const base::State*>, double> distances;
00316 for (unsigned int i = 0 ; i < states.size() ; ++i)
00317 for (unsigned int j = i + 2 ; j < states.size() ; ++j)
00318 distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
00319
00320 bool result = false;
00321 unsigned int nochange = 0;
00322 for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
00323 {
00324
00325 double minDist = std::numeric_limits<double>::infinity();
00326 int p1 = -1;
00327 int p2 = -1;
00328 for (unsigned int i = 0 ; i < states.size() ; ++i)
00329 for (unsigned int j = i + 2 ; j < states.size() ; ++j)
00330 {
00331 double d = distances[std::make_pair(states[i], states[j])];
00332 if (d < minDist)
00333 {
00334 minDist = d;
00335 p1 = i;
00336 p2 = j;
00337 }
00338 }
00339
00340 if (p1 >= 0 && p2 >= 0)
00341 {
00342 if (si->checkMotion(states[p1], states[p2]))
00343 {
00344 for (int i = p1 + 1 ; i < p2 ; ++i)
00345 si->freeState(states[i]);
00346 states.erase(states.begin() + p1 + 1, states.begin() + p2);
00347 result = true;
00348 nochange = 0;
00349 }
00350 else
00351 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
00352 }
00353 else
00354 break;
00355 }
00356 return result;
00357 }
00358
00359 void ompl::geometric::PathSimplifier::simplifyMax(PathGeometric &path)
00360 {
00361 reduceVertices(path);
00362 collapseCloseVertices(path);
00363 smoothBSpline(path, 4, path.length()/100.0);
00364 const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00365 if (!p.second)
00366 logWarn("Solution path may slightly touch on an invalid region of the state space");
00367 else
00368 if (!p.first)
00369 logDebug("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00370 }
00371
00372 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime)
00373 {
00374 simplify(path, base::timedPlannerTerminationCondition(maxTime));
00375 }
00376
00377 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
00378 {
00379 if (path.getStateCount() < 3)
00380 return;
00381
00382
00383 bool tryMore = false;
00384 if (ptc() == false)
00385 tryMore = reduceVertices(path);
00386
00387
00388 if (ptc() == false)
00389 collapseCloseVertices(path);
00390
00391
00392 int times = 0;
00393 while (tryMore && ptc() == false && ++times <= 5)
00394 tryMore = reduceVertices(path);
00395
00396
00397 if (ptc() == false)
00398 tryMore = shortcutPath(path);
00399 else
00400 tryMore = false;
00401
00402
00403 times = 0;
00404 while (tryMore && ptc() == false && ++times <= 5)
00405 tryMore = shortcutPath(path);
00406
00407
00408 if (ptc() == false)
00409 smoothBSpline(path, 3, path.length()/100.0);
00410
00411
00412 const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00413 if (!p.second)
00414 logWarn("Solution path may slightly touch on an invalid region of the state space");
00415 else
00416 if (!p.first)
00417 logDebug("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00418 }