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