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 #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(void)
00058 {
00059 if (!stateValidityChecker_)
00060 {
00061 stateValidityChecker_.reset(new AllValidStateValidityChecker(this));
00062 logWarn("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(void) 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(void)
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(void)
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
00165 if (!satisfiesBounds(state))
00166 enforceBounds(state);
00167
00168 bool result = isValid(state);
00169
00170 if (!result)
00171 {
00172
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
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
00201 count++;
00202
00203 if (count < 2)
00204 {
00205 unsigned int added = 0;
00206
00207
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
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
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(void) 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
00358
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 logError(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 }