ompl/base/src/SpaceInformation.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 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/base/SpaceInformation.h" 00038 #include "ompl/base/samplers/UniformValidStateSampler.h" 00039 #include "ompl/base/DiscreteMotionValidator.h" 00040 #include "ompl/base/spaces/ReedsSheppStateSpace.h" 00041 #include "ompl/base/spaces/DubinsStateSpace.h" 00042 #include "ompl/util/Exception.h" 00043 #include "ompl/util/Time.h" 00044 #include "ompl/tools/config/MagicConstants.h" 00045 #include <queue> 00046 #include <cassert> 00047 00048 ompl::base::SpaceInformation::SpaceInformation(const StateSpacePtr &space) : 00049 stateSpace_(space), setup_(false) 00050 { 00051 if (!stateSpace_) 00052 throw Exception("Invalid space definition"); 00053 setDefaultMotionValidator(); 00054 params_.include(stateSpace_->params()); 00055 } 00056 00057 void ompl::base::SpaceInformation::setup() 00058 { 00059 if (!stateValidityChecker_) 00060 { 00061 stateValidityChecker_.reset(new AllValidStateValidityChecker(this)); 00062 OMPL_WARN("State validity checker not set! No collision checking is performed"); 00063 } 00064 00065 if (!motionValidator_) 00066 setDefaultMotionValidator(); 00067 00068 stateSpace_->setup(); 00069 if (stateSpace_->getDimension() <= 0) 00070 throw Exception("The dimension of the state space we plan in must be > 0"); 00071 00072 params_.clear(); 00073 params_.include(stateSpace_->params()); 00074 00075 setup_ = true; 00076 } 00077 00078 bool ompl::base::SpaceInformation::isSetup() const 00079 { 00080 return setup_; 00081 } 00082 00083 void ompl::base::SpaceInformation::setStateValidityChecker(const StateValidityCheckerFn &svc) 00084 { 00085 class BoostFnStateValidityChecker : public StateValidityChecker 00086 { 00087 public: 00088 00089 BoostFnStateValidityChecker(SpaceInformation *si, 00090 const StateValidityCheckerFn &fn) : StateValidityChecker(si), fn_(fn) 00091 { 00092 } 00093 00094 virtual bool isValid(const State *state) const 00095 { 00096 return fn_(state); 00097 } 00098 00099 protected: 00100 00101 StateValidityCheckerFn fn_; 00102 }; 00103 00104 if (!svc) 00105 throw Exception("Invalid function definition for state validity checking"); 00106 00107 setStateValidityChecker(StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(new BoostFnStateValidityChecker(this, svc)))); 00108 } 00109 00110 void ompl::base::SpaceInformation::setDefaultMotionValidator() 00111 { 00112 if (dynamic_cast<ReedsSheppStateSpace*>(stateSpace_.get())) 00113 motionValidator_.reset(new ReedsSheppMotionValidator(this)); 00114 else if (dynamic_cast<DubinsStateSpace*>(stateSpace_.get())) 00115 motionValidator_.reset(new DubinsMotionValidator(this)); 00116 else 00117 motionValidator_.reset(new DiscreteMotionValidator(this)); 00118 } 00119 00120 00121 void ompl::base::SpaceInformation::setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa) 00122 { 00123 vssa_ = vssa; 00124 setup_ = false; 00125 } 00126 00127 void ompl::base::SpaceInformation::clearValidStateSamplerAllocator() 00128 { 00129 vssa_ = ValidStateSamplerAllocator(); 00130 setup_ = false; 00131 } 00132 00133 unsigned int ompl::base::SpaceInformation::randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const 00134 { 00135 if (alloc) 00136 { 00137 states.resize(steps); 00138 for (unsigned int i = 0 ; i < steps ; ++i) 00139 states[i] = allocState(); 00140 } 00141 else 00142 if (states.size() < steps) 00143 steps = states.size(); 00144 00145 const State *prev = start; 00146 std::pair<State*, double> lastValid; 00147 unsigned int j = 0; 00148 for (unsigned int i = 0 ; i < steps ; ++i) 00149 { 00150 sss->sampleUniform(states[j]); 00151 lastValid.first = states[j]; 00152 if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon()) 00153 prev = states[j++]; 00154 } 00155 00156 return j; 00157 } 00158 00159 bool ompl::base::SpaceInformation::searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const 00160 { 00161 if (state != near) 00162 copyState(state, near); 00163 00164 // fix bounds, if needed 00165 if (!satisfiesBounds(state)) 00166 enforceBounds(state); 00167 00168 bool result = isValid(state); 00169 00170 if (!result) 00171 { 00172 // try to find a valid state nearby 00173 State *temp = cloneState(state); 00174 result = sampler->sampleNear(state, temp, distance); 00175 freeState(temp); 00176 } 00177 00178 return result; 00179 } 00180 00181 bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const 00182 { 00183 if (satisfiesBounds(near) && isValid(near)) 00184 { 00185 if (state != near) 00186 copyState(state, near); 00187 return true; 00188 } 00189 else 00190 { 00191 // try to find a valid state nearby 00192 UniformValidStateSampler *uvss = new UniformValidStateSampler(this); 00193 uvss->setNrAttempts(attempts); 00194 return searchValidNearby(ValidStateSamplerPtr(uvss), state, near, distance); 00195 } 00196 } 00197 00198 unsigned int ompl::base::SpaceInformation::getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const 00199 { 00200 // add 1 to the number of states we want to add between s1 & s2. This gives us the number of segments to split the motion into 00201 count++; 00202 00203 if (count < 2) 00204 { 00205 unsigned int added = 0; 00206 00207 // if they want endpoints, then at most endpoints are included 00208 if (endpoints) 00209 { 00210 if (alloc) 00211 { 00212 states.resize(2); 00213 states[0] = allocState(); 00214 states[1] = allocState(); 00215 } 00216 if (states.size() > 0) 00217 { 00218 copyState(states[0], s1); 00219 added++; 00220 } 00221 00222 if (states.size() > 1) 00223 { 00224 copyState(states[1], s2); 00225 added++; 00226 } 00227 } 00228 else 00229 if (alloc) 00230 states.resize(0); 00231 return added; 00232 } 00233 00234 if (alloc) 00235 { 00236 states.resize(count + (endpoints ? 1 : -1)); 00237 if (endpoints) 00238 states[0] = allocState(); 00239 } 00240 00241 unsigned int added = 0; 00242 00243 if (endpoints && states.size() > 0) 00244 { 00245 copyState(states[0], s1); 00246 added++; 00247 } 00248 00249 /* find the states in between */ 00250 for (unsigned int j = 1 ; j < count && added < states.size() ; ++j) 00251 { 00252 if (alloc) 00253 states[added] = allocState(); 00254 stateSpace_->interpolate(s1, s2, (double)j / (double)count, states[added]); 00255 added++; 00256 } 00257 00258 if (added < states.size() && endpoints) 00259 { 00260 if (alloc) 00261 states[added] = allocState(); 00262 copyState(states[added], s2); 00263 added++; 00264 } 00265 00266 return added; 00267 } 00268 00269 00270 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const 00271 { 00272 assert(states.size() >= count); 00273 for (unsigned int i = 0 ; i < count ; ++i) 00274 if (!isValid(states[i])) 00275 { 00276 firstInvalidStateIndex = i; 00277 return false; 00278 } 00279 return true; 00280 } 00281 00282 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count) const 00283 { 00284 assert(states.size() >= count); 00285 if (count > 0) 00286 { 00287 if (count > 1) 00288 { 00289 if (!isValid(states.front())) 00290 return false; 00291 if (!isValid(states[count - 1])) 00292 return false; 00293 00294 // we have 2 or more states, and the first and last states are valid 00295 00296 if (count > 2) 00297 { 00298 std::queue< std::pair<int, int> > pos; 00299 pos.push(std::make_pair(0, count - 1)); 00300 00301 while (!pos.empty()) 00302 { 00303 std::pair<int, int> x = pos.front(); 00304 00305 int mid = (x.first + x.second) / 2; 00306 if (!isValid(states[mid])) 00307 return false; 00308 00309 if (x.first < mid - 1) 00310 pos.push(std::make_pair(x.first, mid)); 00311 if (x.second > mid + 1) 00312 pos.push(std::make_pair(mid, x.second)); 00313 } 00314 } 00315 } 00316 else 00317 return isValid(states.front()); 00318 } 00319 return true; 00320 } 00321 00322 ompl::base::ValidStateSamplerPtr ompl::base::SpaceInformation::allocValidStateSampler() const 00323 { 00324 if (vssa_) 00325 return vssa_(this); 00326 else 00327 return ValidStateSamplerPtr(new UniformValidStateSampler(this)); 00328 } 00329 00330 double ompl::base::SpaceInformation::probabilityOfValidState(unsigned int attempts) const 00331 { 00332 if (attempts == 0) 00333 return 0.0; 00334 00335 unsigned int valid = 0; 00336 unsigned int invalid = 0; 00337 00338 StateSamplerPtr ss = allocStateSampler(); 00339 State *s = allocState(); 00340 00341 for (unsigned int i = 0 ; i < attempts ; ++i) 00342 { 00343 ss->sampleUniform(s); 00344 if (isValid(s)) 00345 ++valid; 00346 else 00347 ++invalid; 00348 } 00349 00350 freeState(s); 00351 00352 return (double)valid / (double)(valid + invalid); 00353 } 00354 00355 double ompl::base::SpaceInformation::averageValidMotionLength(unsigned int attempts) const 00356 { 00357 // take the square root here because we in fact have a nested for loop 00358 // where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop too) 00359 attempts = std::max((unsigned int)floor(sqrt((double)attempts) + 0.5), 2u); 00360 00361 StateSamplerPtr ss = allocStateSampler(); 00362 UniformValidStateSampler *uvss = new UniformValidStateSampler(this); 00363 uvss->setNrAttempts(attempts); 00364 00365 State *s1 = allocState(); 00366 State *s2 = allocState(); 00367 00368 std::pair<State*, double> lastValid; 00369 lastValid.first = NULL; 00370 00371 double d = 0.0; 00372 unsigned int count = 0; 00373 for (unsigned int i = 0 ; i < attempts ; ++i) 00374 if (uvss->sample(s1)) 00375 { 00376 ++count; 00377 ss->sampleUniform(s2); 00378 if (checkMotion(s1, s2, lastValid)) 00379 d += distance(s1, s2); 00380 else 00381 d += distance(s1, s2) * lastValid.second; 00382 } 00383 00384 freeState(s2); 00385 freeState(s1); 00386 delete uvss; 00387 00388 if (count > 0) 00389 return d / (double)count; 00390 else 00391 return 0.0; 00392 } 00393 00394 void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const 00395 { 00396 StateSamplerPtr ss = allocStateSampler(); 00397 std::vector<State*> states(attempts + 1); 00398 allocStates(states); 00399 00400 time::point start = time::now(); 00401 for (unsigned int i = 0 ; i < attempts ; ++i) 00402 ss->sampleUniform(states[i]); 00403 uniform = (double)attempts / time::seconds(time::now() - start); 00404 00405 double d = getMaximumExtent() / 10.0; 00406 ss->sampleUniform(states[attempts]); 00407 00408 start = time::now(); 00409 for (unsigned int i = 1 ; i <= attempts ; ++i) 00410 ss->sampleUniformNear(states[i - 1], states[i], d); 00411 near = (double)attempts / time::seconds(time::now() - start); 00412 00413 start = time::now(); 00414 for (unsigned int i = 1 ; i <= attempts ; ++i) 00415 ss->sampleGaussian(states[i - 1], states[i], d); 00416 gaussian = (double)attempts / time::seconds(time::now() - start); 00417 00418 freeStates(states); 00419 } 00420 00421 void ompl::base::SpaceInformation::printSettings(std::ostream &out) const 00422 { 00423 out << "Settings for the state space '" << stateSpace_->getName() << "'" << std::endl; 00424 out << " - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%' << std::endl; 00425 out << " - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl; 00426 out << " - state space:" << std::endl; 00427 stateSpace_->printSettings(out); 00428 out << std::endl << "Declared parameters:" << std::endl; 00429 params_.print(out); 00430 ValidStateSamplerPtr vss = allocValidStateSampler(); 00431 out << "Valid state sampler named " << vss->getName() << " with parameters:" << std::endl; 00432 vss->params().print(out); 00433 } 00434 00435 void ompl::base::SpaceInformation::printProperties(std::ostream &out) const 00436 { 00437 out << "Properties of the state space '" << stateSpace_->getName() << "'" << std::endl; 00438 out << " - signature: "; 00439 std::vector<int> sig; 00440 stateSpace_->computeSignature(sig); 00441 for (std::size_t i = 0 ; i < sig.size() ; ++i) 00442 out << sig[i] << " "; 00443 out << std::endl; 00444 out << " - dimension: " << stateSpace_->getDimension() << std::endl; 00445 out << " - extent: " << stateSpace_->getMaximumExtent() << std::endl; 00446 if (isSetup()) 00447 { 00448 bool result = true; 00449 try 00450 { 00451 stateSpace_->sanityChecks(); 00452 } 00453 catch(Exception &e) 00454 { 00455 result = false; 00456 out << std::endl << " - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl << std::endl; 00457 OMPL_ERROR(e.what()); 00458 } 00459 if (result) 00460 out << " - sanity checks for state space passed" << std::endl; 00461 out << " - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl; 00462 out << " - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT) << std::endl; 00463 double uniform, near, gaussian; 00464 samplesPerSecond(uniform, near, gaussian, magic::TEST_STATE_COUNT); 00465 out << " - average number of samples drawn per second: sampleUniform()=" << uniform << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl; 00466 } 00467 else 00468 out << "Call setup() before to get more information" << std::endl; 00469 }