WrapperStateSpace.h
155 }
206 }
211 }
216 }
267 }
271 space_->copyState(destination->as<StateType>()->getState(), source->as<StateType>()->getState());
272 }
276 return space_->distance(state1->as<StateType>()->getState(), state2->as<StateType>()->getState());
277 }
282 }
296 return space_->equalStates(state1->as<StateType>()->getState(), state2->as<StateType>()->getState());
301 return space_->interpolate(from->as<StateType>()->getState(), to->as<StateType>()->getState(), t,
303 }
308 }
313 }
318 space_->freeState(wstate->getState());
374 space_->registerProjections();
unsigned int validSegmentCount(const State *state1, const State *state2) const override
Count how many segments of the "longest valid length" fit on the motion from state1 to state2.
Definition: WrapperStateSpace.h:282
bool isDiscrete() const override
Check if the set of states is discrete.
Definition: WrapperStateSpace.h:201
void copyToReals(std::vector< double > &reals, const State *source) const override
Copy all the real values from a state source to the array reals using getValueAddressAtLocation()
Definition: WrapperStateSpace.h:426
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
Definition: WrapperStateSpace.cpp:67
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
Wrapper state type. Contains a reference to an underlying state.
Definition: WrapperStateSpace.h:167
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
Definition: WrapperStateSpace.h:348
StateType(State *state)
Constructor. Takes a reference state to the underlying state.
Definition: WrapperStateSpace.h:171
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
Definition: WrapperStateSpace.h:446
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
Definition: WrapperStateSpace.h:471
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
Definition: WrapperStateSpace.h:466
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
Definition: WrapperStateSpace.h:338
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
Definition: WrapperStateSpace.h:386
unsigned int getValidSegmentCountFactor() const override
Get the value used to multiply the return value of validSegmentCount().
Definition: WrapperStateSpace.h:293
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a nearby state using underlying sampler.
Definition: WrapperStateSpace.cpp:44
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
Definition: WrapperStateSpace.h:206
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
Definition: WrapperStateSpace.h:436
void setValidSegmentCountFactor(unsigned int factor) override
Set factor to be the value to multiply the return value of validSegmentCount(). By default,...
Definition: WrapperStateSpace.h:288
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
Definition: WrapperStateSpace.h:318
State * allocState() const override
Allocate a state that can store a point in the described space.
Definition: WrapperStateSpace.h:374
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
void copyFromReals(State *destination, const std::vector< double > &reals) const override
Copy the values from reals to the state destination using getValueAddressAtLocation()
Definition: WrapperStateSpace.h:431
bool hasSymmetricInterpolate() const override
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from,...
Definition: WrapperStateSpace.h:221
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
Definition: WrapperStateSpace.cpp:72
void computeSignature(std::vector< int > &signature) const override
Compute an array of ints that uniquely identifies the structure of the state space....
Definition: WrapperStateSpace.h:303
WrapperStateSampler(const StateSpace *space, StateSamplerPtr sampler)
Constructor. Requires the wrapper state space space and the underlying sampler sampler.
Definition: WrapperStateSpace.h:155
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
Definition: WrapperStateSpace.h:369
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
Definition: WrapperStateSpace.h:353
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state within a Gaussian distribution using underlying sampler.
Definition: WrapperStateSpace.cpp:50
void freeState(State *state) const override
Free the memory of the allocated state.
Definition: WrapperStateSpace.h:379
double getLongestValidSegmentLength() const override
Get the longest valid segment at the time setup() was called.
Definition: WrapperStateSpace.h:298
const State * getState() const
Get a const pointer to the underlying state.
Definition: WrapperStateSpace.h:176
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: WrapperStateSpace.h:277
bool satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
Definition: WrapperStateSpace.h:328
void printProjections(std::ostream &out) const override
Print the list of registered projections. This function is also called by printSettings()
Definition: WrapperStateSpace.h:451
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
Definition: WrapperStateSpace.h:343
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
Definition: WrapperStateSpace.cpp:77
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
Definition: WrapperStateSpace.h:363
void enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
Definition: WrapperStateSpace.h:323
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
Definition: WrapperStateSpace.h:308
void sanityChecks() const override
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition: WrapperStateSpace.h:461
A shared pointer wrapper for ompl::base::StateSpace.
double getLongestValidSegmentFraction() const override
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: WrapperStateSpace.h:272
A shared pointer wrapper for ompl::base::StateSampler.
void printState(const State *state, std::ostream &out=std::cout) const override
Print a state to a stream.
Definition: WrapperStateSpace.h:441
bool isMetricSpace() const override
Return true if the distance function associated with the space is a metric.
Definition: WrapperStateSpace.h:211
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Definition: WrapperStateSpace.h:333
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
Definition: WrapperStateSpace.h:358
bool isCompound() const override
Check if the state space is compound.
Definition: WrapperStateSpace.h:196
void sampleUniform(State *state) override
Sample a state using underlying sampler.
Definition: WrapperStateSpace.cpp:39
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Definition: WrapperStateSpace.h:313
bool hasSymmetricDistance() const override
Check if the distance function on this state space is symmetric, i.e. distance(s1,...
Definition: WrapperStateSpace.h:216