ompl/geometric/src/PathGeometric.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, 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 Willow Garage 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/PathGeometric.h" 00038 #include "ompl/base/samplers/UniformValidStateSampler.h" 00039 #include "ompl/base/OptimizationObjective.h" 00040 #include "ompl/base/ScopedState.h" 00041 #include <algorithm> 00042 #include <cmath> 00043 #include <limits> 00044 #include <boost/math/constants/constants.hpp> 00045 00046 ompl::geometric::PathGeometric::PathGeometric(const PathGeometric &path) : base::Path(path.si_) 00047 { 00048 copyFrom(path); 00049 } 00050 00051 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state) : base::Path(si) 00052 { 00053 states_.resize(1); 00054 states_[0] = si_->cloneState(state); 00055 } 00056 00057 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1, const base::State *state2) : base::Path(si) 00058 { 00059 states_.resize(2); 00060 states_[0] = si_->cloneState(state1); 00061 states_[1] = si_->cloneState(state2); 00062 } 00063 00064 ompl::geometric::PathGeometric& ompl::geometric::PathGeometric::operator=(const PathGeometric &other) 00065 { 00066 if (this != &other) 00067 { 00068 freeMemory(); 00069 si_ = other.si_; 00070 copyFrom(other); 00071 } 00072 return *this; 00073 } 00074 00075 void ompl::geometric::PathGeometric::copyFrom(const PathGeometric &other) 00076 { 00077 states_.resize(other.states_.size()); 00078 for (unsigned int i = 0 ; i < states_.size() ; ++i) 00079 states_[i] = si_->cloneState(other.states_[i]); 00080 } 00081 00082 void ompl::geometric::PathGeometric::freeMemory() 00083 { 00084 for (unsigned int i = 0 ; i < states_.size() ; ++i) 00085 si_->freeState(states_[i]); 00086 } 00087 00088 ompl::base::Cost ompl::geometric::PathGeometric::cost(const base::OptimizationObjectivePtr &opt) const 00089 { 00090 if (states_.empty()) return opt->identityCost(); 00091 // Compute path cost by accumulating the cost along the path 00092 base::Cost cost(opt->initialCost(states_.front())); 00093 for (std::size_t i = 1; i < states_.size(); ++i) 00094 cost = opt->combineCosts(cost, opt->motionCost(states_[i - 1], states_[i])); 00095 cost = opt->combineCosts(cost, opt->terminalCost(states_.back())); 00096 return cost; 00097 } 00098 00099 double ompl::geometric::PathGeometric::length() const 00100 { 00101 double L = 0.0; 00102 for (unsigned int i = 1 ; i < states_.size() ; ++i) 00103 L += si_->distance(states_[i-1], states_[i]); 00104 return L; 00105 } 00106 00107 double ompl::geometric::PathGeometric::clearance() const 00108 { 00109 double c = 0.0; 00110 for (unsigned int i = 0 ; i < states_.size() ; ++i) 00111 c += si_->getStateValidityChecker()->clearance(states_[i]); 00112 if (states_.empty()) 00113 c = std::numeric_limits<double>::infinity(); 00114 else 00115 c /= (double)states_.size(); 00116 return c; 00117 } 00118 00119 double ompl::geometric::PathGeometric::smoothness() const 00120 { 00121 double s = 0.0; 00122 if (states_.size() > 2) 00123 { 00124 double a = si_->distance(states_[0], states_[1]); 00125 for (unsigned int i = 2 ; i < states_.size() ; ++i) 00126 { 00127 // view the path as a sequence of segments, and look at the triangles it forms: 00128 // s1 00129 // /\ s4 00130 // a / \ b | 00131 // / \ | 00132 // /......\_______| 00133 // s0 c s2 s3 00134 // 00135 // use Pythagoras generalized theorem to find the cos of the angle between segments a and b 00136 double b = si_->distance(states_[i-1], states_[i]); 00137 double c = si_->distance(states_[i-2], states_[i]); 00138 double acosValue = (a*a + b*b - c*c) / (2.0*a*b); 00139 00140 if (acosValue > -1.0 && acosValue < 1.0) 00141 { 00142 // the smoothness is actually the outside angle of the one we compute 00143 double angle = (boost::math::constants::pi<double>() - acos(acosValue)); 00144 00145 // and we normalize by the length of the segments 00146 double k = 2.0 * angle / (a + b); 00147 s += k * k; 00148 } 00149 a = b; 00150 } 00151 } 00152 return s; 00153 } 00154 00155 bool ompl::geometric::PathGeometric::check() const 00156 { 00157 bool result = true; 00158 if (states_.size() > 0) 00159 { 00160 if (si_->isValid(states_[0])) 00161 { 00162 int last = states_.size() - 1; 00163 for (int j = 0 ; result && j < last ; ++j) 00164 if (!si_->checkMotion(states_[j], states_[j + 1])) 00165 result = false; 00166 } 00167 else 00168 result = false; 00169 } 00170 00171 return result; 00172 } 00173 00174 void ompl::geometric::PathGeometric::print(std::ostream &out) const 00175 { 00176 out << "Geometric path with " << states_.size() << " states" << std::endl; 00177 for (unsigned int i = 0 ; i < states_.size() ; ++i) 00178 si_->printState(states_[i], out); 00179 out << std::endl; 00180 } 00181 void ompl::geometric::PathGeometric::printAsMatrix(std::ostream &out) const 00182 { 00183 const base::StateSpace* space(si_->getStateSpace().get()); 00184 std::vector<double> reals; 00185 for (unsigned int i = 0 ; i < states_.size() ; ++i) 00186 { 00187 space->copyToReals(reals, states_[i]); 00188 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " ")); 00189 out << std::endl; 00190 } 00191 out << std::endl; 00192 } 00193 00194 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts) 00195 { 00196 if (states_.empty()) 00197 return std::make_pair(true, true); 00198 if (states_.size() == 1) 00199 { 00200 bool result = si_->isValid(states_[0]); 00201 return std::make_pair(result, result); 00202 } 00203 00204 // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway 00205 const int n1 = states_.size() - 1; 00206 if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1])) 00207 return std::make_pair(false, false); 00208 00209 base::State *temp = NULL; 00210 base::UniformValidStateSampler *uvss = NULL; 00211 bool result = true; 00212 00213 for (int i = 1 ; i < n1 ; ++i) 00214 if (!si_->checkMotion(states_[i-1], states_[i])) 00215 { 00216 // we now compute a state around which to sample 00217 if (!temp) 00218 temp = si_->allocState(); 00219 if (!uvss) 00220 { 00221 uvss = new base::UniformValidStateSampler(si_.get()); 00222 uvss->setNrAttempts(attempts); 00223 } 00224 00225 // and a radius of sampling around that state 00226 double radius = 0.0; 00227 00228 if (si_->isValid(states_[i])) 00229 { 00230 si_->copyState(temp, states_[i]); 00231 radius = si_->distance(states_[i-1], states_[i]); 00232 } 00233 else 00234 { 00235 unsigned int nextValid = n1; 00236 for (int j = i + 1 ; j < n1 ; ++j) 00237 if (si_->isValid(states_[j])) 00238 { 00239 nextValid = j; 00240 break; 00241 } 00242 // we know nextValid will be initialised because n1 is certainly valid. 00243 si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp); 00244 radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i])); 00245 } 00246 00247 bool success = false; 00248 00249 for (unsigned int a = 0 ; a < attempts ; ++a) 00250 if (uvss->sampleNear(states_[i], temp, radius)) 00251 { 00252 if (si_->checkMotion(states_[i-1], states_[i])) 00253 { 00254 success = true; 00255 break; 00256 } 00257 } 00258 else 00259 break; 00260 if (!success) 00261 { 00262 result = false; 00263 break; 00264 } 00265 } 00266 00267 // free potentially allocated memory 00268 if (temp) 00269 si_->freeState(temp); 00270 bool originalValid = uvss == NULL; 00271 if (uvss) 00272 delete uvss; 00273 00274 return std::make_pair(originalValid, result); 00275 } 00276 00277 void ompl::geometric::PathGeometric::subdivide() 00278 { 00279 if (states_.size() < 2) 00280 return; 00281 std::vector<base::State*> newStates(1, states_[0]); 00282 for (unsigned int i = 1 ; i < states_.size() ; ++i) 00283 { 00284 base::State *temp = si_->allocState(); 00285 si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp); 00286 newStates.push_back(temp); 00287 newStates.push_back(states_[i]); 00288 } 00289 states_.swap(newStates); 00290 } 00291 00292 void ompl::geometric::PathGeometric::interpolate() 00293 { 00294 unsigned int n = 0; 00295 const int n1 = states_.size() - 1; 00296 for (int i = 0 ; i < n1 ; ++i) 00297 n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]); 00298 interpolate(n); 00299 } 00300 00301 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount) 00302 { 00303 if (requestCount < states_.size() || states_.size() < 2) 00304 return; 00305 00306 unsigned int count = requestCount; 00307 00308 // the remaining length of the path we need to add states along 00309 double remainingLength = length(); 00310 00311 // the new array of states this path will have 00312 std::vector<base::State*> newStates; 00313 const int n1 = states_.size() - 1; 00314 00315 for (int i = 0 ; i < n1 ; ++i) 00316 { 00317 base::State *s1 = states_[i]; 00318 base::State *s2 = states_[i + 1]; 00319 00320 newStates.push_back(s1); 00321 00322 // the maximum number of states that can be added on the current motion (without its endpoints) 00323 // such that we can at least fit the remaining states 00324 int maxNStates = count + i - states_.size(); 00325 00326 if (maxNStates > 0) 00327 { 00328 // compute an approximate number of states the following segment needs to contain; this includes endpoints 00329 double segmentLength = si_->distance(s1, s2); 00330 int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1; 00331 00332 // if more than endpoints are needed 00333 if (ns > 2) 00334 { 00335 ns -= 2; // subtract endpoints 00336 00337 // make sure we don't add too many states 00338 if (ns > maxNStates) 00339 ns = maxNStates; 00340 00341 // compute intermediate states 00342 std::vector<base::State*> block; 00343 unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true); 00344 // sanity checks 00345 if ((int)ans != ns || block.size() != ans) 00346 throw Exception("Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers."); 00347 00348 newStates.insert(newStates.end(), block.begin(), block.end()); 00349 } 00350 else 00351 ns = 0; 00352 00353 // update what remains to be done 00354 count -= (ns + 1); 00355 remainingLength -= segmentLength; 00356 } 00357 else 00358 count--; 00359 } 00360 00361 // add the last state 00362 newStates.push_back(states_[n1]); 00363 states_.swap(newStates); 00364 if (requestCount != states_.size()) 00365 throw Exception("Internal error in path interpolation. This should never happen. Please contact the developers."); 00366 } 00367 00368 void ompl::geometric::PathGeometric::reverse() 00369 { 00370 std::reverse(states_.begin(), states_.end()); 00371 } 00372 00373 void ompl::geometric::PathGeometric::random() 00374 { 00375 freeMemory(); 00376 states_.resize(2); 00377 states_[0] = si_->allocState(); 00378 states_[1] = si_->allocState(); 00379 base::StateSamplerPtr ss = si_->allocStateSampler(); 00380 ss->sampleUniform(states_[0]); 00381 ss->sampleUniform(states_[1]); 00382 } 00383 00384 bool ompl::geometric::PathGeometric::randomValid(unsigned int attempts) 00385 { 00386 freeMemory(); 00387 states_.resize(2); 00388 states_[0] = si_->allocState(); 00389 states_[1] = si_->allocState(); 00390 base::UniformValidStateSampler *uvss = new base::UniformValidStateSampler(si_.get()); 00391 uvss->setNrAttempts(attempts); 00392 bool ok = false; 00393 for (unsigned int i = 0 ; i < attempts ; ++i) 00394 { 00395 if (uvss->sample(states_[0]) && uvss->sample(states_[1])) 00396 if (si_->checkMotion(states_[0], states_[1])) 00397 { 00398 ok = true; 00399 break; 00400 } 00401 } 00402 delete uvss; 00403 if (!ok) 00404 { 00405 freeMemory(); 00406 states_.clear(); 00407 } 00408 return ok; 00409 } 00410 00411 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex) 00412 { 00413 if (startIndex > states_.size()) 00414 throw Exception("Index on path is out of bounds"); 00415 const base::StateSpacePtr &sm = over.si_->getStateSpace(); 00416 const base::StateSpacePtr &dm = si_->getStateSpace(); 00417 bool copy = !states_.empty(); 00418 for (unsigned int i = 0, j = startIndex ; i < over.states_.size() ; ++i, ++j) 00419 { 00420 if (j == states_.size()) 00421 { 00422 base::State *s = si_->allocState(); 00423 if (copy) 00424 si_->copyState(s, states_.back()); 00425 states_.push_back(s); 00426 } 00427 00428 copyStateData(dm, states_[j], sm, over.states_[i]); 00429 } 00430 } 00431 00432 void ompl::geometric::PathGeometric::append(const base::State *state) 00433 { 00434 states_.push_back(si_->cloneState(state)); 00435 } 00436 00437 void ompl::geometric::PathGeometric::append(const PathGeometric &path) 00438 { 00439 if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName()) 00440 { 00441 PathGeometric copy(path); 00442 states_.insert(states_.end(), copy.states_.begin(), copy.states_.end()); 00443 copy.states_.clear(); 00444 } 00445 else 00446 overlay(path, states_.size()); 00447 } 00448 00449 void ompl::geometric::PathGeometric::prepend(const base::State *state) 00450 { 00451 states_.insert(states_.begin(), si_->cloneState(state)); 00452 } 00453 00454 void ompl::geometric::PathGeometric::keepAfter(const base::State *state) 00455 { 00456 int index = getClosestIndex(state); 00457 if (index > 0) 00458 { 00459 if ((std::size_t)(index + 1) < states_.size()) 00460 { 00461 double b = si_->distance(state, states_[index-1]); 00462 double a = si_->distance(state, states_[index+1]); 00463 if (b > a) 00464 ++index; 00465 } 00466 for (int i = 0 ; i < index ; ++i) 00467 si_->freeState(states_[i]); 00468 states_.erase(states_.begin(), states_.begin() + index); 00469 } 00470 } 00471 00472 void ompl::geometric::PathGeometric::keepBefore(const base::State *state) 00473 { 00474 int index = getClosestIndex(state); 00475 if (index >= 0) 00476 { 00477 if (index > 0 && (std::size_t)(index + 1) < states_.size()) 00478 { 00479 double b = si_->distance(state, states_[index-1]); 00480 double a = si_->distance(state, states_[index+1]); 00481 if (b < a) 00482 --index; 00483 } 00484 if ((std::size_t)(index + 1) < states_.size()) 00485 { 00486 for (std::size_t i = index + 1 ; i < states_.size() ; ++i) 00487 si_->freeState(states_[i]); 00488 states_.resize(index + 1); 00489 } 00490 } 00491 } 00492 00493 int ompl::geometric::PathGeometric::getClosestIndex(const base::State *state) const 00494 { 00495 if (states_.empty()) 00496 return -1; 00497 00498 int index = 0; 00499 double min_d = si_->distance(states_[0], state); 00500 for (std::size_t i = 1 ; i < states_.size() ; ++i) 00501 { 00502 double d = si_->distance(states_[i], state); 00503 if (d < min_d) 00504 { 00505 min_d = d; 00506 index = i; 00507 } 00508 } 00509 return index; 00510 }