SpaceInformation.h
311 bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;
332 unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps,
403 void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const;
unsigned int getStateDimension() const
Return the dimension of the state space.
Definition: SpaceInformation.h:210
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Definition: SpaceInformation.h:100
void setStateValidityCheckingResolution(double resolution)
Set the resolution at which state validity needs to be verified in order for a motion between two sta...
Definition: SpaceInformation.h:192
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.
MotionValidatorPtr motionValidator_
The instance of the motion validator to use when determining the validity of motions in the planning ...
Definition: SpaceInformation.h:445
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
MotionValidatorPtr & getMotionValidator()
Return the non-const instance of the used state validity checker.
Definition: SpaceInformation.h:181
StateSamplerPtr allocStateSampler() const
Allocate a uniform state sampler for the state space.
Definition: SpaceInformation.h:268
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 isValid(const State *state) const
Check if a given state is valid or not.
Definition: SpaceInformation.h:94
const MotionValidatorPtr & getMotionValidator() const
Return the instance of the used state validity checker.
Definition: SpaceInformation.h:175
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
double getStateValidityCheckingResolution() const
Get the resolution at which state validity is verified. This call is only applicable if a ompl::base:...
Definition: SpaceInformation.h:202
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
bool equalStates(const State *state1, const State *state2) const
Check if two states are the same.
Definition: SpaceInformation.h:109
ParamSet & params()
Get the combined parameters for the classes that the space information manages.
Definition: SpaceInformation.h:412
A shared pointer wrapper for ompl::base::StateValidityChecker.
const ParamSet & params() const
Get the combined parameters for the classes that the space information manages.
Definition: SpaceInformation.h:418
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
void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
Definition: SpaceInformation.h:133
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
Definition: SpaceInformation.h:115
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
Definition: SpaceInformation.cpp:426
A shared pointer wrapper for ompl::base::MotionValidator.
void copyState(State *destination, const State *source) const
Copy a state to another.
Definition: SpaceInformation.h:251
const StateValidityCheckerPtr & getStateValidityChecker() const
Return the instance of the used state validity checker.
Definition: SpaceInformation.h:160
bool setup_
Flag indicating whether setup() has been called on this instance.
Definition: SpaceInformation.h:448
void enforceBounds(State *state) const
Bring the state within the bounds of the state space.
Definition: SpaceInformation.h:127
unsigned int getCheckedMotionCount() const
Get the total number of motion segments checked by the MotionValidator so far.
Definition: SpaceInformation.h:385
StateValidityCheckerPtr stateValidityChecker_
The instance of the state validity checker used for determining the validity of states in the plannin...
Definition: SpaceInformation.h:441
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
void freeStates(std::vector< State *> &states) const
Free the memory of an array of states.
Definition: SpaceInformation.h:244
bool checkMotion(const State *s1, const State *s2) const
Check if the path between two states (from s1 to s2) is valid, using the MotionValidator. This function assumes s1 is valid.
Definition: SpaceInformation.h:352
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
void clearValidStateSamplerAllocator()
Clear the allocator used for the valid state sampler. This will revert to using the uniform valid sta...
Definition: SpaceInformation.cpp:124
ValidStateSamplerAllocator vssa_
The optional valid state sampler allocator.
Definition: SpaceInformation.h:451
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:297
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
void allocStates(std::vector< State *> &states) const
Allocate memory for each element of the array states.
Definition: SpaceInformation.h:231
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
void setMotionValidator(const MotionValidatorPtr &mv)
Set the instance of the motion validity checker to use. Parallel implementations of planners assume t...
Definition: SpaceInformation.h:168
double distance(const State *state1, const State *state2) const
Compute the distance between two states.
Definition: SpaceInformation.h:121
double getSpaceMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...
Definition: SpaceInformation.h:216