ompl/geometric/src/PathSimplifier.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Rice University, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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 bool ompl::geometric::PathSimplifier::freeStates() const 00046 { 00047 return freeStates_; 00048 } 00049 00050 void ompl::geometric::PathSimplifier::freeStates(bool flag) 00051 { 00052 freeStates_ = flag; 00053 } 00054 00055 /* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */ 00056 void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange) 00057 { 00058 if (path.getStateCount() < 3) 00059 return; 00060 00061 const base::SpaceInformationPtr &si = path.getSpaceInformation(); 00062 std::vector<base::State*> &states = path.getStates(); 00063 00064 base::State *temp1 = si->allocState(); 00065 base::State *temp2 = si->allocState(); 00066 00067 for (unsigned int s = 0 ; s < maxSteps ; ++s) 00068 { 00069 path.subdivide(); 00070 00071 unsigned int i = 2, u = 0, n1 = states.size() - 1; 00072 while (i < n1) 00073 { 00074 if (si->isValid(states[i - 1])) 00075 { 00076 si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1); 00077 si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2); 00078 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1); 00079 if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1])) 00080 { 00081 if (si->distance(states[i], temp1) > minChange) 00082 { 00083 si->copyState(states[i], temp1); 00084 ++u; 00085 } 00086 } 00087 } 00088 00089 i += 2; 00090 } 00091 00092 if (u == 0) 00093 break; 00094 } 00095 00096 si->freeState(temp1); 00097 si->freeState(temp2); 00098 } 00099 00100 bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio) 00101 { 00102 if (path.getStateCount() < 3) 00103 return false; 00104 00105 if (maxSteps == 0) 00106 maxSteps = path.getStateCount(); 00107 00108 if (maxEmptySteps == 0) 00109 maxEmptySteps = path.getStateCount(); 00110 00111 bool result = false; 00112 unsigned int nochange = 0; 00113 const base::SpaceInformationPtr &si = path.getSpaceInformation(); 00114 std::vector<base::State*> &states = path.getStates(); 00115 00116 if (si->checkMotion(states.front(), states.back())) 00117 { 00118 if (freeStates_) 00119 for (std::size_t i = 2 ; i < states.size() ; ++i) 00120 si->freeState(states[i-1]); 00121 std::vector<base::State*> newStates(2); 00122 newStates[0] = states.front(); 00123 newStates[1] = states.back(); 00124 states.swap(newStates); 00125 result = true; 00126 } 00127 else 00128 for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange) 00129 { 00130 int count = states.size(); 00131 int maxN = count - 1; 00132 int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio)); 00133 00134 int p1 = rng_.uniformInt(0, maxN); 00135 int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range)); 00136 if (abs(p1 - p2) < 2) 00137 { 00138 if (p1 < maxN - 1) 00139 p2 = p1 + 2; 00140 else 00141 if (p1 > 1) 00142 p2 = p1 - 2; 00143 else 00144 continue; 00145 } 00146 00147 if (p1 > p2) 00148 std::swap(p1, p2); 00149 00150 if (si->checkMotion(states[p1], states[p2])) 00151 { 00152 if (freeStates_) 00153 for (int j = p1 + 1 ; j < p2 ; ++j) 00154 si->freeState(states[j]); 00155 states.erase(states.begin() + p1 + 1, states.begin() + p2); 00156 nochange = 0; 00157 result = true; 00158 } 00159 } 00160 return result; 00161 } 00162 00163 bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex) 00164 { 00165 if (path.getStateCount() < 3) 00166 return false; 00167 00168 if (maxSteps == 0) 00169 maxSteps = path.getStateCount(); 00170 00171 if (maxEmptySteps == 0) 00172 maxEmptySteps = path.getStateCount(); 00173 00174 const base::SpaceInformationPtr &si = path.getSpaceInformation(); 00175 std::vector<base::State*> &states = path.getStates(); 00176 00177 std::vector<double> dists(states.size(), 0.0); 00178 for (unsigned int i = 1 ; i < dists.size() ; ++i) 00179 dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]); 00180 double threshold = dists.back() * snapToVertex; 00181 double rd = rangeRatio * dists.back(); 00182 00183 base::State *temp0 = si->allocState(); 00184 base::State *temp1 = si->allocState(); 00185 bool result = false; 00186 unsigned int nochange = 0; 00187 for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange) 00188 { 00189 base::State *s0 = NULL; 00190 int index0 = -1; 00191 double t0 = 0.0; 00192 double p0 = rng_.uniformReal(0.0, dists.back()); 00193 std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0); 00194 int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin(); 00195 00196 if (pos0 == 0 || dists[pos0] - p0 < threshold) 00197 index0 = pos0; 00198 else 00199 { 00200 while (pos0 > 0 && p0 < dists[pos0]) 00201 --pos0; 00202 if (p0 - dists[pos0] < threshold) 00203 index0 = pos0; 00204 } 00205 00206 base::State *s1 = NULL; 00207 int index1 = -1; 00208 double t1 = 0.0; 00209 double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back())); 00210 pit = std::lower_bound(dists.begin(), dists.end(), p1); 00211 int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin(); 00212 00213 if (pos1 == 0 || dists[pos1] - p1 < threshold) 00214 index1 = pos1; 00215 else 00216 { 00217 while (pos1 > 0 && p1 < dists[pos1]) 00218 --pos1; 00219 if (p1 - dists[pos1] < threshold) 00220 index1 = pos1; 00221 } 00222 00223 if (pos0 == pos1 || index0 == pos1 || index1 == pos0 || 00224 pos0 + 1 == index1 || pos1 + 1 == index0 || 00225 (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2)) 00226 continue; 00227 00228 if (index0 >= 0) 00229 s0 = states[index0]; 00230 else 00231 { 00232 t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]); 00233 si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0); 00234 s0 = temp0; 00235 } 00236 00237 if (index1 >= 0) 00238 s1 = states[index1]; 00239 else 00240 { 00241 t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]); 00242 si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1); 00243 s1 = temp1; 00244 } 00245 00246 if (si->checkMotion(s0, s1)) 00247 { 00248 if (pos0 > pos1) 00249 { 00250 std::swap(pos0, pos1); 00251 std::swap(index0, index1); 00252 std::swap(s0, s1); 00253 std::swap(t0, t1); 00254 } 00255 00256 if (index0 < 0 && index1 < 0) 00257 { 00258 if (pos0 + 1 == pos1) 00259 { 00260 si->copyState(states[pos1], s0); 00261 states.insert(states.begin() + pos1 + 1, si->cloneState(s1)); 00262 } 00263 else 00264 { 00265 if (freeStates_) 00266 for (int j = pos0 + 2 ; j < pos1 ; ++j) 00267 si->freeState(states[j]); 00268 si->copyState(states[pos0 + 1], s0); 00269 si->copyState(states[pos1], s1); 00270 states.erase(states.begin() + pos0 + 2, states.begin() + pos1); 00271 } 00272 } 00273 else 00274 if (index0 >= 0 && index1 >= 0) 00275 { 00276 if (freeStates_) 00277 for (int j = index0 + 1 ; j < index1 ; ++j) 00278 si->freeState(states[j]); 00279 states.erase(states.begin() + index0 + 1, states.begin() + index1); 00280 } 00281 else 00282 if (index0 < 0 && index1 >= 0) 00283 { 00284 if (freeStates_) 00285 for (int j = pos0 + 2 ; j < index1 ; ++j) 00286 si->freeState(states[j]); 00287 si->copyState(states[pos0 + 1], s0); 00288 states.erase(states.begin() + pos0 + 2, states.begin() + index1); 00289 } 00290 else 00291 if (index0 >= 0 && index1 < 0) 00292 { 00293 if (freeStates_) 00294 for (int j = index0 + 1 ; j < pos1 ; ++j) 00295 si->freeState(states[j]); 00296 si->copyState(states[pos1], s1); 00297 states.erase(states.begin() + index0 + 1, states.begin() + pos1); 00298 } 00299 00300 // fix the helper variables 00301 dists.resize(states.size(), 0.0); 00302 for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j) 00303 dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]); 00304 threshold = dists.back() * snapToVertex; 00305 rd = rangeRatio * dists.back(); 00306 result = true; 00307 nochange = 0; 00308 } 00309 } 00310 00311 si->freeState(temp1); 00312 si->freeState(temp0); 00313 return result; 00314 } 00315 00316 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps) 00317 { 00318 if (path.getStateCount() < 3) 00319 return false; 00320 00321 if (maxSteps == 0) 00322 maxSteps = path.getStateCount(); 00323 00324 if (maxEmptySteps == 0) 00325 maxEmptySteps = path.getStateCount(); 00326 00327 const base::SpaceInformationPtr &si = path.getSpaceInformation(); 00328 std::vector<base::State*> &states = path.getStates(); 00329 00330 // compute pair-wise distances in path (construct only half the matrix) 00331 std::map<std::pair<const base::State*, const base::State*>, double> distances; 00332 for (unsigned int i = 0 ; i < states.size() ; ++i) 00333 for (unsigned int j = i + 2 ; j < states.size() ; ++j) 00334 distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]); 00335 00336 bool result = false; 00337 unsigned int nochange = 0; 00338 for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange) 00339 { 00340 // find closest pair of points 00341 double minDist = std::numeric_limits<double>::infinity(); 00342 int p1 = -1; 00343 int p2 = -1; 00344 for (unsigned int i = 0 ; i < states.size() ; ++i) 00345 for (unsigned int j = i + 2 ; j < states.size() ; ++j) 00346 { 00347 double d = distances[std::make_pair(states[i], states[j])]; 00348 if (d < minDist) 00349 { 00350 minDist = d; 00351 p1 = i; 00352 p2 = j; 00353 } 00354 } 00355 00356 if (p1 >= 0 && p2 >= 0) 00357 { 00358 if (si->checkMotion(states[p1], states[p2])) 00359 { 00360 if (freeStates_) 00361 for (int i = p1 + 1 ; i < p2 ; ++i) 00362 si->freeState(states[i]); 00363 states.erase(states.begin() + p1 + 1, states.begin() + p2); 00364 result = true; 00365 nochange = 0; 00366 } 00367 else 00368 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity(); 00369 } 00370 else 00371 break; 00372 } 00373 return result; 00374 } 00375 00376 void ompl::geometric::PathSimplifier::simplifyMax(PathGeometric &path) 00377 { 00378 reduceVertices(path); 00379 collapseCloseVertices(path); 00380 // BSpline and checkAndRepair code only makes sense in a metric space. 00381 if (si_->getStateSpace()->isMetricSpace()) 00382 { 00383 smoothBSpline(path, 4, path.length()/100.0); 00384 const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS); 00385 if (!p.second) 00386 OMPL_WARN("Solution path may slightly touch on an invalid region of the state space"); 00387 else 00388 if (!p.first) 00389 OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed."); 00390 } 00391 } 00392 00393 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime) 00394 { 00395 simplify(path, base::timedPlannerTerminationCondition(maxTime)); 00396 } 00397 00398 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc) 00399 { 00400 if (path.getStateCount() < 3) 00401 return; 00402 00403 // try a randomized step of connecting vertices 00404 bool tryMore = false; 00405 if (ptc == false) 00406 tryMore = reduceVertices(path); 00407 00408 // try to collapse close-by vertices 00409 if (ptc == false) 00410 collapseCloseVertices(path); 00411 00412 // try to reduce verices some more, if there is any point in doing so 00413 int times = 0; 00414 while (tryMore && ptc == false && ++times <= 5) 00415 tryMore = reduceVertices(path); 00416 00417 // if the space is metric, we can do some additional smoothing 00418 if(si_->getStateSpace()->isMetricSpace()) 00419 { 00420 // run a more complex short-cut algorithm : allow splitting path segments 00421 if (ptc == false) 00422 tryMore = shortcutPath(path); 00423 else 00424 tryMore = false; 00425 00426 // run the short-cut algorithm some more, if it makes a difference 00427 times = 0; 00428 while (tryMore && ptc == false && ++times <= 5) 00429 tryMore = shortcutPath(path); 00430 00431 // smooth the path with BSpline interpolation 00432 if(ptc == false) 00433 smoothBSpline(path, 3, path.length()/100.0); 00434 00435 // we always run this if the metric-space algorithms were run. In non-metric spaces this does not work. 00436 const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS); 00437 if (!p.second) 00438 OMPL_WARN("Solution path may slightly touch on an invalid region of the state space"); 00439 else 00440 if (!p.first) 00441 OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed."); 00442 } 00443 }