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;
double distance(const State *state1, const State *state2) const
Compute the distance between two states.
Definition: SpaceInformation.h:125
void print(std::ostream &out) const
Print the parameters to a stream.
Definition: GenericParam.cpp:161
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
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
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.
void allocStates(std::vector< State * > &states) const
Allocate memory for each element of the array states.
Definition: SpaceInformation.h:224
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.
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
Definition: SpaceInformation.cpp:421
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
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
bool isValid(const State *state) const
Check if a given state is valid or not.
Definition: SpaceInformation.h:98
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
A motion validator that only uses the state validity checker. Motions are checked for validity at a s...
Definition: DiscreteMotionValidator.h:50
double averageValidMotionLength(unsigned int attempts) const
Estimate the length of a valid motion. setup() is assumed to have been called.
Definition: SpaceInformation.cpp:355
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
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
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
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
Definition: SpaceInformation.h:119
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
ValidStateSamplerPtr allocValidStateSampler() const
Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() was ...
Definition: SpaceInformation.cpp:322
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
void copyState(State *destination, const State *source) const
Copy a state to another.
Definition: SpaceInformation.h:244
virtual void printProperties(std::ostream &out=std::cout) const
Print properties of the current instance of the state space.
Definition: SpaceInformation.cpp:435
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
bool setup_
Flag indicating whether setup() has been called on this instance.
Definition: SpaceInformation.h:415
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
void freeStates(std::vector< State * > &states) const
Free the memory of an array of states.
Definition: SpaceInformation.h:237
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Definition: StateValidityChecker.h:93
StateSamplerPtr allocStateSampler() const
Allocate a uniform state sampler for the state space.
Definition: SpaceInformation.h:264
double probabilityOfValidState(unsigned int attempts) const
Estimate probability of sampling a valid state. setup() is assumed to have been called.
Definition: SpaceInformation.cpp:330
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
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
SpaceInformation(const StateSpacePtr &space)
Constructor. Sets the instance of the state space to plan with.
Definition: SpaceInformation.cpp:48
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 enforceBounds(State *state) const
Bring the state within the bounds of the state space.
Definition: SpaceInformation.h:131
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
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...
Definition: ValidStateSampler.h:96