ompl/base/SpaceInformation.h
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 #ifndef OMPL_BASE_SPACE_INFORMATION_ 00038 #define OMPL_BASE_SPACE_INFORMATION_ 00039 00040 #include "ompl/base/State.h" 00041 #include "ompl/base/StateValidityChecker.h" 00042 #include "ompl/base/MotionValidator.h" 00043 #include "ompl/base/StateSpace.h" 00044 #include "ompl/base/ValidStateSampler.h" 00045 00046 #include "ompl/util/ClassForward.h" 00047 #include "ompl/util/Console.h" 00048 #include "ompl/util/Exception.h" 00049 00050 #include <boost/noncopyable.hpp> 00051 #include <boost/function.hpp> 00052 #include <boost/bind.hpp> 00053 00054 #include <utility> 00055 #include <cstdlib> 00056 #include <vector> 00057 #include <iostream> 00058 00060 namespace ompl 00061 { 00062 00067 namespace base 00068 { 00070 00071 OMPL_CLASS_FORWARD(SpaceInformation); 00073 00080 typedef boost::function<bool(const State*)> StateValidityCheckerFn; 00081 00082 00086 class SpaceInformation : private boost::noncopyable 00087 { 00088 public: 00089 00091 SpaceInformation(const StateSpacePtr &space); 00092 00093 virtual ~SpaceInformation() 00094 { 00095 } 00096 00098 bool isValid(const State *state) const 00099 { 00100 return stateValidityChecker_->isValid(state); 00101 } 00102 00104 const StateSpacePtr& getStateSpace() const 00105 { 00106 return stateSpace_; 00107 } 00108 00113 bool equalStates(const State *state1, const State *state2) const 00114 { 00115 return stateSpace_->equalStates(state1, state2); 00116 } 00117 00119 bool satisfiesBounds(const State *state) const 00120 { 00121 return stateSpace_->satisfiesBounds(state); 00122 } 00123 00125 double distance(const State *state1, const State *state2) const 00126 { 00127 return stateSpace_->distance(state1, state2); 00128 } 00129 00131 void enforceBounds(State *state) const 00132 { 00133 stateSpace_->enforceBounds(state); 00134 } 00135 00137 void printState(const State *state, std::ostream &out = std::cout) const 00138 { 00139 stateSpace_->printState(state, out); 00140 } 00141 00150 void setStateValidityChecker(const StateValidityCheckerPtr &svc) 00151 { 00152 stateValidityChecker_ = svc; 00153 setup_ = false; 00154 } 00155 00161 void setStateValidityChecker(const StateValidityCheckerFn &svc); 00162 00164 const StateValidityCheckerPtr& getStateValidityChecker() const 00165 { 00166 return stateValidityChecker_; 00167 } 00168 00172 void setMotionValidator(const MotionValidatorPtr &mv) 00173 { 00174 motionValidator_ = mv; 00175 setup_ = false; 00176 } 00177 00179 const MotionValidatorPtr& getMotionValidator() const 00180 { 00181 return motionValidator_; 00182 } 00183 00190 void setStateValidityCheckingResolution(double resolution) 00191 { 00192 stateSpace_->setLongestValidSegmentFraction(resolution); 00193 setup_ = false; 00194 } 00195 00200 double getStateValidityCheckingResolution() const 00201 { 00202 return stateSpace_->getLongestValidSegmentFraction(); 00203 } 00204 00205 00209 unsigned int getStateDimension() const 00210 { 00211 return stateSpace_->getDimension(); 00212 } 00213 00218 State* allocState() const 00219 { 00220 return stateSpace_->allocState(); 00221 } 00222 00224 void allocStates(std::vector<State*> &states) const 00225 { 00226 for (unsigned int i = 0 ; i < states.size() ; ++i) 00227 states[i] = stateSpace_->allocState(); 00228 } 00229 00231 void freeState(State *state) const 00232 { 00233 stateSpace_->freeState(state); 00234 } 00235 00237 void freeStates(std::vector<State*> &states) const 00238 { 00239 for (unsigned int i = 0 ; i < states.size() ; ++i) 00240 stateSpace_->freeState(states[i]); 00241 } 00242 00244 void copyState(State *destination, const State *source) const 00245 { 00246 stateSpace_->copyState(destination, source); 00247 } 00248 00250 State* cloneState(const State *source) const 00251 { 00252 State *copy = stateSpace_->allocState(); 00253 stateSpace_->copyState(copy, source); 00254 return copy; 00255 } 00256 00264 StateSamplerPtr allocStateSampler() const 00265 { 00266 return stateSpace_->allocStateSampler(); 00267 } 00268 00272 ValidStateSamplerPtr allocValidStateSampler() const; 00273 00276 void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa); 00277 00279 void clearValidStateSamplerAllocator(); 00280 00289 double getMaximumExtent() const 00290 { 00291 return stateSpace_->getMaximumExtent(); 00292 } 00293 00301 bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const; 00302 00310 bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const; 00311 00318 unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const; 00319 00326 bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const 00327 { 00328 return motionValidator_->checkMotion(s1, s2, lastValid); 00329 } 00330 00331 00333 bool checkMotion(const State *s1, const State *s2) const 00334 { 00335 return motionValidator_->checkMotion(s1, s2); 00336 } 00337 00343 bool checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const; 00344 00346 bool checkMotion(const std::vector<State*> &states, unsigned int count) const; 00347 00358 unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const; 00359 00366 double probabilityOfValidState(unsigned int attempts) const; 00367 00369 double averageValidMotionLength(unsigned int attempts) const; 00370 00372 void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const; 00373 00375 virtual void printSettings(std::ostream &out = std::cout) const; 00376 00378 virtual void printProperties(std::ostream &out = std::cout) const; 00379 00381 ParamSet& params() 00382 { 00383 return params_; 00384 } 00385 00387 const ParamSet& params() const 00388 { 00389 return params_; 00390 } 00391 00396 virtual void setup(); 00397 00399 bool isSetup() const; 00400 00401 protected: 00403 void setDefaultMotionValidator(); 00404 00406 StateSpacePtr stateSpace_; 00407 00409 StateValidityCheckerPtr stateValidityChecker_; 00410 00412 MotionValidatorPtr motionValidator_; 00413 00415 bool setup_; 00416 00418 ValidStateSamplerAllocator vssa_; 00419 00421 ParamSet params_; 00422 }; 00423 00424 } 00425 00426 } 00427 00428 #endif