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 #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 ClassForward(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(void)
00094 {
00095 }
00096
00098 bool isValid(const State *state) const
00099 {
00100 return stateValidityChecker_->isValid(state);
00101 }
00102
00104 const StateSpacePtr& getStateSpace(void) 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(void) 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(void) 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(void) const
00201 {
00202 return stateSpace_->getLongestValidSegmentFraction();
00203 }
00204
00205
00209 unsigned int getStateDimension(void) const
00210 {
00211 return stateSpace_->getDimension();
00212 }
00213
00218 State* allocState(void) 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(void) const
00265 {
00266 return stateSpace_->allocStateSampler();
00267 }
00268
00272 ValidStateSamplerPtr allocValidStateSampler(void) const;
00273
00276 void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa);
00277
00279 void clearValidStateSamplerAllocator(void);
00280
00289 double getMaximumExtent(void) 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
00356 unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const;
00357
00364 double probabilityOfValidState(unsigned int attempts) const;
00365
00367 double averageValidMotionLength(unsigned int attempts) const;
00368
00370 void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const;
00371
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(void)
00382 {
00383 return params_;
00384 }
00385
00387 const ParamSet& params(void) const
00388 {
00389 return params_;
00390 }
00391
00396 virtual void setup(void);
00397
00399 bool isSetup(void) const;
00400
00401 protected:
00403 void setDefaultMotionValidator(void);
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