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