37 #include "ompl/base/SpaceInformation.h"
38 #include "ompl/base/samplers/UniformValidStateSampler.h"
39 #include "ompl/base/DiscreteMotionValidator.h"
40 #include "ompl/base/spaces/ReedsSheppStateSpace.h"
41 #include "ompl/base/spaces/DubinsStateSpace.h"
42 #include "ompl/util/Exception.h"
43 #include "ompl/util/Time.h"
44 #include "ompl/tools/config/MagicConstants.h"
49 stateSpace_(space), setup_(false)
52 throw Exception(
"Invalid space definition");
59 if (!stateValidityChecker_)
62 logWarn(
"State validity checker not set! No collision checking is performed");
65 if (!motionValidator_)
66 setDefaultMotionValidator();
69 if (stateSpace_->getDimension() <= 0)
70 throw Exception(
"The dimension of the state space we plan in must be > 0");
73 params_.include(stateSpace_->params());
94 virtual bool isValid(
const State *state)
const
105 throw Exception(
"Invalid function definition for state validity checking");
107 setStateValidityChecker(
StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(
new BoostFnStateValidityChecker(
this, svc))));
112 if (dynamic_cast<ReedsSheppStateSpace*>(stateSpace_.get()))
114 else if (dynamic_cast<DubinsStateSpace*>(stateSpace_.get()))
137 states.resize(steps);
138 for (
unsigned int i = 0 ; i < steps ; ++i)
139 states[i] = allocState();
142 if (states.size() < steps)
143 steps = states.size();
145 const State *prev = start;
146 std::pair<State*, double> lastValid;
148 for (
unsigned int i = 0 ; i < steps ; ++i)
150 sss->sampleUniform(states[j]);
151 lastValid.first = states[j];
152 if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
162 copyState(state, near);
165 if (!satisfiesBounds(state))
166 enforceBounds(state);
168 bool result = isValid(state);
173 State *temp = cloneState(state);
174 result = sampler->sampleNear(state, temp, distance);
183 if (satisfiesBounds(near) && isValid(near))
186 copyState(state, near);
205 unsigned int added = 0;
213 states[0] = allocState();
214 states[1] = allocState();
216 if (states.size() > 0)
218 copyState(states[0], s1);
222 if (states.size() > 1)
224 copyState(states[1], s2);
236 states.resize(count + (endpoints ? 1 : -1));
238 states[0] = allocState();
241 unsigned int added = 0;
243 if (endpoints && states.size() > 0)
245 copyState(states[0], s1);
250 for (
unsigned int j = 1 ; j < count && added < states.size() ; ++j)
253 states[added] = allocState();
254 stateSpace_->interpolate(s1, s2, (
double)j / (
double)count, states[added]);
258 if (added < states.size() && endpoints)
261 states[added] = allocState();
262 copyState(states[added], s2);
272 assert(states.size() >= count);
273 for (
unsigned int i = 0 ; i < count ; ++i)
274 if (!isValid(states[i]))
276 firstInvalidStateIndex = i;
284 assert(states.size() >= count);
289 if (!isValid(states.front()))
291 if (!isValid(states[count - 1]))
298 std::queue< std::pair<int, int> > pos;
299 pos.push(std::make_pair(0, count - 1));
303 std::pair<int, int> x = pos.front();
305 int mid = (x.first + x.second) / 2;
306 if (!isValid(states[mid]))
309 if (x.first < mid - 1)
310 pos.push(std::make_pair(x.first, mid));
311 if (x.second > mid + 1)
312 pos.push(std::make_pair(mid, x.second));
317 return isValid(states.front());
335 unsigned int valid = 0;
336 unsigned int invalid = 0;
339 State *s = allocState();
341 for (
unsigned int i = 0 ; i < attempts ; ++i)
343 ss->sampleUniform(s);
352 return (
double)valid / (double)(valid + invalid);
359 attempts = std::max((
unsigned int)floor(sqrt((
double)attempts) + 0.5), 2u);
365 State *s1 = allocState();
366 State *s2 = allocState();
368 std::pair<State*, double> lastValid;
369 lastValid.first = NULL;
372 unsigned int count = 0;
373 for (
unsigned int i = 0 ; i < attempts ; ++i)
377 ss->sampleUniform(s2);
378 if (checkMotion(s1, s2, lastValid))
379 d += distance(s1, s2);
381 d += distance(s1, s2) * lastValid.second;
389 return d / (double)count;
397 std::vector<State*> states(attempts + 1);
401 for (
unsigned int i = 0 ; i < attempts ; ++i)
402 ss->sampleUniform(states[i]);
405 double d = getMaximumExtent() / 10.0;
406 ss->sampleUniform(states[attempts]);
409 for (
unsigned int i = 1 ; i <= attempts ; ++i)
410 ss->sampleUniformNear(states[i - 1], states[i], d);
414 for (
unsigned int i = 1 ; i <= attempts ; ++i)
415 ss->sampleGaussian(states[i - 1], states[i], d);
423 out <<
"Settings for the state space '" << stateSpace_->getName() <<
"'" << std::endl;
424 out <<
" - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) <<
'%' << std::endl;
425 out <<
" - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl;
426 out <<
" - state space:" << std::endl;
427 stateSpace_->printSettings(out);
428 out << std::endl <<
"Declared parameters:" << std::endl;
431 out <<
"Valid state sampler named " << vss->getName() <<
" with parameters:" << std::endl;
432 vss->params().print(out);
437 out <<
"Properties of the state space '" << stateSpace_->getName() <<
"'" << std::endl;
438 out <<
" - signature: ";
439 std::vector<int> sig;
440 stateSpace_->computeSignature(sig);
441 for (std::size_t i = 0 ; i < sig.size() ; ++i)
442 out << sig[i] <<
" ";
444 out <<
" - dimension: " << stateSpace_->getDimension() << std::endl;
445 out <<
" - extent: " << stateSpace_->getMaximumExtent() << std::endl;
451 stateSpace_->sanityChecks();
456 out << std::endl <<
" - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() <<
")" << std::endl << std::endl;
460 out <<
" - sanity checks for state space passed" << std::endl;
462 out <<
" - average length of a valid motion: " << averageValidMotionLength(
magic::TEST_STATE_COUNT) << std::endl;
463 double uniform, near, gaussian;
465 out <<
" - average number of samples drawn per second: sampleUniform()=" << uniform <<
" sampleUniformNear()=" << near <<
" sampleGaussian()=" << gaussian << std::endl;
468 out <<
"Call setup() before to get more information" << std::endl;