SpaceInformation.cpp
107 setStateValidityChecker(StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(new BoostFnStateValidityChecker(this, svc))));
121 void ompl::base::SpaceInformation::setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
133 unsigned int ompl::base::SpaceInformation::randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const
152 if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
159 bool ompl::base::SpaceInformation::searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const
181 bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
198 unsigned int ompl::base::SpaceInformation::getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const
200 // add 1 to the number of states we want to add between s1 & s2. This gives us the number of segments to split the motion into
270 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const
282 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count) const
358 // where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop too)
394 void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const
424 out << " - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%' << std::endl;
425 out << " - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl;
456 out << std::endl << " - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl << std::endl;
461 out << " - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
462 out << " - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT) << std::endl;
465 out << " - average number of samples drawn per second: sampleUniform()=" << uniform << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl;
boost::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
Definition: ValidStateSampler.h:135
virtual bool sample(State *state)
Sample a state. Return false in case of failure.
Definition: UniformValidStateSampler.cpp:46
void include(const ParamSet &other, const std::string &prefix="")
Include the params of a different ParamSet into this one. Optionally include a prefix for each of the...
Definition: GenericParam.cpp:135
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
double probabilityOfValidState(unsigned int attempts) const
Estimate probability of sampling a valid state. setup() is assumed to have been called.
Definition: SpaceInformation.cpp:330
double averageValidMotionLength(unsigned int attempts) const
Estimate the length of a valid motion. setup() is assumed to have been called.
Definition: SpaceInformation.cpp:355
A boost shared pointer wrapper for ompl::base::StateSpace.
MotionValidatorPtr motionValidator_
The instance of the motion validator to use when determining the validity of motions in the planning ...
Definition: SpaceInformation.h:412
A boost shared pointer wrapper for ompl::base::StateSampler.
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
Definition: SpaceInformation.h:150
StateSamplerPtr allocStateSampler() const
Allocate a uniform state sampler for the state space.
Definition: SpaceInformation.h:264
void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const
Estimate the number of samples that can be drawn per second, using the sampler returned by allocState...
Definition: SpaceInformation.cpp:394
A motion validator that only uses the state validity checker. Motions are checked for validity at a s...
Definition: DiscreteMotionValidator.h:50
bool isValid(const State *state) const
Check if a given state is valid or not.
Definition: SpaceInformation.h:98
bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
Find a valid state near a given one. If the given state is valid, it will be returned itself...
Definition: SpaceInformation.cpp:181
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
Definition: SpaceInformation.h:80
ValidStateSamplerPtr allocValidStateSampler() const
Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() was ...
Definition: SpaceInformation.cpp:322
double getStateValidityCheckingResolution() const
Get the resolution at which state validity is verified. This call is only applicable if a ompl::base:...
Definition: SpaceInformation.h:200
unsigned int getMotionStates(const State *s1, const State *s2, std::vector< State *> &states, unsigned int count, bool endpoints, bool alloc) const
Get count states that make up a motion between s1 and s2. Returns the number of states that were adde...
Definition: SpaceInformation.cpp:198
A state sampler that only samples valid states, uniformly.
Definition: UniformValidStateSampler.h:50
void setDefaultMotionValidator()
Set default motion validator for the state space.
Definition: SpaceInformation.cpp:110
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
The simplest state validity checker: all states are valid.
Definition: StateValidityChecker.h:166
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
StateSpacePtr stateSpace_
The state space planning is to be performed in.
Definition: SpaceInformation.h:406
void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
Set the allocator to use for a valid state sampler. This replaces the default uniform valid state sam...
Definition: SpaceInformation.cpp:121
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
Definition: SpaceInformation.h:119
void print(std::ostream &out) const
Print the parameters to a stream.
Definition: GenericParam.cpp:161
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
Definition: SpaceInformation.cpp:421
void copyState(State *destination, const State *source) const
Copy a state to another.
Definition: SpaceInformation.h:244
bool setup_
Flag indicating whether setup() has been called on this instance.
Definition: SpaceInformation.h:415
void enforceBounds(State *state) const
Bring the state within the bounds of the state space.
Definition: SpaceInformation.h:131
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Definition: StateValidityChecker.h:93
A Reeds-Shepp motion validator that only uses the state validity checker. Motions are checked for val...
Definition: ReedsSheppStateSpace.h:125
StateValidityCheckerPtr stateValidityChecker_
The instance of the state validity checker used for determining the validity of states in the plannin...
Definition: SpaceInformation.h:409
The base class for space information. This contains all the information about the space planning is d...
Definition: SpaceInformation.h:86
virtual void setup()
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
Definition: SpaceInformation.cpp:57
A Dubins motion validator that only uses the state validity checker. Motions are checked for validity...
Definition: DubinsStateSpace.h:164
SpaceInformation(const StateSpacePtr &space)
Constructor. Sets the instance of the state space to plan with.
Definition: SpaceInformation.cpp:48
virtual void printProperties(std::ostream &out=std::cout) const
Print properties of the current instance of the state space.
Definition: SpaceInformation.cpp:435
void freeStates(std::vector< State *> &states) const
Free the memory of an array of states.
Definition: SpaceInformation.h:237
bool checkMotion(const State *s1, const State *s2, std::pair< State *, double > &lastValid) const
Incrementally check if the path between two motions is valid. Also compute the last state that was va...
Definition: SpaceInformation.h:326
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
Definition: MagicConstants.h:109
void clearValidStateSamplerAllocator()
Clear the allocator used for the valid state sampler. This will revert to using the uniform valid sta...
Definition: SpaceInformation.cpp:127
ValidStateSamplerAllocator vssa_
The optional valid state sampler allocator.
Definition: SpaceInformation.h:418
double getMaximumExtent() const
Get the maximum extent of the space we are planning in. This is the maximum distance that could be re...
Definition: SpaceInformation.h:289
unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector< State *> &states, bool alloc) const
Produce a valid motion starting at start by randomly bouncing off of invalid states. The start state start is not included in the computed motion (states). Returns the number of elements written to states (less or equal to steps).
Definition: SpaceInformation.cpp:133
void allocStates(std::vector< State *> &states) const
Allocate memory for each element of the array states.
Definition: SpaceInformation.h:224
double distance(const State *state1, const State *state2) const
Compute the distance between two states.
Definition: SpaceInformation.h:125
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...
Definition: ValidStateSampler.h:96