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 for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00107 {
00108 int count = states.size();
00109 int maxN = count - 1;
00110 int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
00111
00112 int p1 = rng_.uniformInt(0, maxN);
00113 int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
00114 if (abs(p1 - p2) < 2)
00115 {
00116 if (p1 < maxN - 1)
00117 p2 = p1 + 2;
00118 else
00119 if (p1 > 1)
00120 p2 = p1 - 2;
00121 else
00122 continue;
00123 }
00124
00125 if (p1 > p2)
00126 std::swap(p1, p2);
00127
00128 if (si->checkMotion(states[p1], states[p2]))
00129 {
00130 for (int j = p1 + 1 ; j < p2 ; ++j)
00131 si->freeState(states[j]);
00132 states.erase(states.begin() + p1 + 1, states.begin() + p2);
00133 nochange = 0;
00134 result = true;
00135 }
00136 }
00137 return result;
00138 }
00139
00140 bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
00141 {
00142 if (path.getStateCount() < 3)
00143 return false;
00144
00145 if (maxSteps == 0)
00146 maxSteps = path.getStateCount();
00147
00148 if (maxEmptySteps == 0)
00149 maxEmptySteps = path.getStateCount();
00150
00151 const base::SpaceInformationPtr &si = path.getSpaceInformation();
00152 std::vector<base::State*> &states = path.getStates();
00153
00154 std::vector<double> dists(states.size(), 0.0);
00155 for (unsigned int i = 1 ; i < dists.size() ; ++i)
00156 dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
00157 double threshold = dists.back() * snapToVertex;
00158 double rd = rangeRatio * dists.back();
00159
00160 base::State *temp0 = si->allocState();
00161 base::State *temp1 = si->allocState();
00162 bool result = false;
00163 unsigned int nochange = 0;
00164 for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00165 {
00166 base::State *s0 = NULL;
00167 int index0 = -1;
00168 double t0 = 0.0;
00169 double p0 = rng_.uniformReal(0.0, dists.back());
00170 std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0);
00171 int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
00172
00173 if (pos0 == 0 || dists[pos0] - p0 < threshold)
00174 index0 = pos0;
00175 else
00176 {
00177 while (pos0 > 0 && p0 < dists[pos0])
00178 --pos0;
00179 if (p0 - dists[pos0] < threshold)
00180 index0 = pos0;
00181 }
00182
00183 base::State *s1 = NULL;
00184 int index1 = -1;
00185 double t1 = 0.0;
00186 double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back()));
00187 pit = std::lower_bound(dists.begin(), dists.end(), p1);
00188 int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
00189
00190 if (pos1 == 0 || dists[pos1] - p1 < threshold)
00191 index1 = pos1;
00192 else
00193 {
00194 while (pos1 > 0 && p1 < dists[pos1])
00195 --pos1;
00196 if (p1 - dists[pos1] < threshold)
00197 index1 = pos1;
00198 }
00199
00200 if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
00201 pos0 + 1 == index1 || pos1 + 1 == index0 ||
00202 (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
00203 continue;
00204
00205 if (index0 >= 0)
00206 s0 = states[index0];
00207 else
00208 {
00209 t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
00210 si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
00211 s0 = temp0;
00212 }
00213
00214 if (index1 >= 0)
00215 s1 = states[index1];
00216 else
00217 {
00218 t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
00219 si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
00220 s1 = temp1;
00221 }
00222
00223 if (si->checkMotion(s0, s1))
00224 {
00225 if (pos0 > pos1)
00226 {
00227 std::swap(pos0, pos1);
00228 std::swap(index0, index1);
00229 std::swap(s0, s1);
00230 std::swap(t0, t1);
00231 }
00232
00233 if (index0 < 0 && index1 < 0)
00234 {
00235 if (pos0 + 1 == pos1)
00236 {
00237 si->copyState(states[pos1], s0);
00238 states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
00239 }
00240 else
00241 {
00242 for (int j = pos0 + 2 ; j < pos1 ; ++j)
00243 si->freeState(states[j]);
00244 si->copyState(states[pos0 + 1], s0);
00245 si->copyState(states[pos1], s1);
00246 states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
00247 }
00248 }
00249 else
00250 if (index0 >= 0 && index1 >= 0)
00251 {
00252 for (int j = index0 + 1 ; j < index1 ; ++j)
00253 si->freeState(states[j]);
00254 states.erase(states.begin() + index0 + 1, states.begin() + index1);
00255 }
00256 else
00257 if (index0 < 0 && index1 >= 0)
00258 {
00259 for (int j = pos0 + 2 ; j < index1 ; ++j)
00260 si->freeState(states[j]);
00261 si->copyState(states[pos0 + 1], s0);
00262 states.erase(states.begin() + pos0 + 2, states.begin() + index1);
00263 }
00264 else
00265 if (index0 >= 0 && index1 < 0)
00266 {
00267 for (int j = index0 + 1 ; j < pos1 ; ++j)
00268 si->freeState(states[j]);
00269 si->copyState(states[pos1], s1);
00270 states.erase(states.begin() + index0 + 1, states.begin() + pos1);
00271 }
00272
00273
00274 dists.resize(states.size(), 0.0);
00275 for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
00276 dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
00277 threshold = dists.back() * snapToVertex;
00278 rd = rangeRatio * dists.back();
00279 result = true;
00280 nochange = 0;
00281 }
00282 }
00283
00284 si->freeState(temp1);
00285 si->freeState(temp0);
00286 return result;
00287 }
00288
00289 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
00290 {
00291 if (path.getStateCount() < 3)
00292 return false;
00293
00294 if (maxSteps == 0)
00295 maxSteps = path.getStateCount();
00296
00297 if (maxEmptySteps == 0)
00298 maxEmptySteps = path.getStateCount();
00299
00300 const base::SpaceInformationPtr &si = path.getSpaceInformation();
00301 std::vector<base::State*> &states = path.getStates();
00302
00303
00304 std::map<std::pair<const base::State*, const base::State*>, double> distances;
00305 for (unsigned int i = 0 ; i < states.size() ; ++i)
00306 for (unsigned int j = i + 2 ; j < states.size() ; ++j)
00307 distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
00308
00309 bool result = false;
00310 unsigned int nochange = 0;
00311 for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
00312 {
00313
00314 double minDist = std::numeric_limits<double>::infinity();
00315 int p1 = -1;
00316 int p2 = -1;
00317 for (unsigned int i = 0 ; i < states.size() ; ++i)
00318 for (unsigned int j = i + 2 ; j < states.size() ; ++j)
00319 {
00320 double d = distances[std::make_pair(states[i], states[j])];
00321 if (d < minDist)
00322 {
00323 minDist = d;
00324 p1 = i;
00325 p2 = j;
00326 }
00327 }
00328
00329 if (p1 >= 0 && p2 >= 0)
00330 {
00331 if (si->checkMotion(states[p1], states[p2]))
00332 {
00333 for (int i = p1 + 1 ; i < p2 ; ++i)
00334 si->freeState(states[i]);
00335 states.erase(states.begin() + p1 + 1, states.begin() + p2);
00336 result = true;
00337 nochange = 0;
00338 }
00339 else
00340 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
00341 }
00342 else
00343 break;
00344 }
00345 return result;
00346 }
00347
00348 void ompl::geometric::PathSimplifier::simplifyMax(PathGeometric &path)
00349 {
00350 reduceVertices(path);
00351 collapseCloseVertices(path);
00352 smoothBSpline(path, 4, path.length()/100.0);
00353 const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00354 if (!p.second)
00355 msg_.warn("Solution path may slightly touch on an invalid region of the state space");
00356 else
00357 if (!p.first)
00358 msg_.debug("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00359 }
00360
00361 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime)
00362 {
00363 simplify(path, base::timedPlannerTerminationCondition(maxTime));
00364 }
00365
00366 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
00367 {
00368 if (path.getStateCount() < 3)
00369 return;
00370
00371
00372 bool tryMore = false;
00373 if (ptc() == false)
00374 tryMore = reduceVertices(path);
00375
00376
00377 if (ptc() == false)
00378 collapseCloseVertices(path);
00379
00380
00381 int times = 0;
00382 while (tryMore && ptc() == false && ++times <= 5)
00383 tryMore = reduceVertices(path);
00384
00385
00386 if (ptc() == false)
00387 tryMore = shortcutPath(path);
00388 else
00389 tryMore = false;
00390
00391
00392 times = 0;
00393 while (tryMore && ptc() == false && ++times <= 5)
00394 tryMore = shortcutPath(path);
00395
00396
00397 if (ptc() == false)
00398 smoothBSpline(path, 3, path.length()/100.0);
00399
00400
00401 const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00402 if (!p.second)
00403 msg_.warn("Solution path may slightly touch on an invalid region of the state space");
00404 else
00405 if (!p.first)
00406 msg_.debug("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00407 }