SpaceInformation.cpp
49 ompl::base::SpaceInformation::SpaceInformation(StateSpacePtr space) : stateSpace_(std::move(space)), setup_(false)
118 void ompl::base::SpaceInformation::setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
130 unsigned int ompl::base::SpaceInformation::randomBounceMotion(const StateSamplerPtr &sss, const State *start,
150 if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
157 bool ompl::base::SpaceInformation::searchValidNearby(const ValidStateSamplerPtr &sampler, State *state,
180 bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance,
202 // add 1 to the number of states we want to add between s1 & s2. This gives us the number of segments to split the
271 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State *> &states, unsigned int count,
284 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State *> &states, unsigned int count) const
362 // where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop
398 void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian,
429 out << " - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%'
431 out << " - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl;
470 out << " - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
471 out << " - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT)
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:134
A 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:334
double averageValidMotionLength(unsigned int attempts) const
Estimate the length of a valid motion. setup() is assumed to have been called.
Definition: SpaceInformation.cpp:359
A shared pointer wrapper for ompl::base::StateSpace.
A 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:146
STL namespace.
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:398
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:180
ValidStateSamplerPtr allocValidStateSampler() const
Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() was ...
Definition: SpaceInformation.cpp:326
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
void setDefaultMotionValidator()
Set default motion validator for the state space.
Definition: SpaceInformation.cpp:108
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
StateSpacePtr stateSpace_
The state space planning is to be performed in.
Definition: SpaceInformation.h:437
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:118
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
Definition: SpaceInformation.cpp:426
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Definition: StateValidityChecker.h:90
The base class for space information. This contains all the information about the space planning is d...
Definition: SpaceInformation.h:81
virtual void setup()
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
Definition: SpaceInformation.cpp:57
std::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
Definition: ValidStateSampler.h:134
virtual void printProperties(std::ostream &out=std::cout) const
Print properties of the current instance of the state space.
Definition: SpaceInformation.cpp:442
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:345
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:101
void clearValidStateSamplerAllocator()
Clear the allocator used for the valid state sampler. This will revert to using the uniform valid sta...
Definition: SpaceInformation.cpp:124
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:130
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
Definition: SpaceInformation.h:76