37 #ifndef OMPL_BASE_STATE_SPACE_
38 #define OMPL_BASE_STATE_SPACE_
40 #include "ompl/base/State.h"
41 #include "ompl/base/StateSpaceTypes.h"
42 #include "ompl/base/StateSampler.h"
43 #include "ompl/base/ProjectionEvaluator.h"
44 #include "ompl/base/GenericParam.h"
46 #include "ompl/util/ClassForward.h"
47 #include <boost/concept_check.hpp>
48 #include <boost/noncopyable.hpp>
61 OMPL_CLASS_FORWARD(StateSpace);
90 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
92 return static_cast<T*
>(
this);
97 const T*
as(
void)
const
100 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
102 return static_cast<const T*
>(
this);
190 const std::string&
getName(
void)
const;
193 void setName(
const std::string &name);
302 virtual void serialize(
void *serialization,
const State *state)
const;
429 virtual void sanityChecks(
double zero,
double eps,
unsigned int flags)
const;
436 void diagram(std::ostream &out)
const;
439 void list(std::ostream &out)
const;
442 static void Diagram(std::ostream &out);
445 static void List(std::ostream &out);
489 virtual void setup(
void);
549 CompoundStateSpace(
const std::vector<StateSpacePtr> &components,
const std::vector<double> &weights);
557 T*
as(
const unsigned int index)
const
560 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
567 T*
as(
const std::string &name)
const
570 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
614 const std::vector<StateSpacePtr>&
getSubspaces(
void)
const;
654 virtual void serialize(
void *serialization,
const State *state)
const;
691 virtual void setup(
void);
772 const StateSpacePtr &sourceS,
const State *source);
782 const StateSpace *sourceS,
const State *source);
790 const StateSpacePtr &sourceS,
const State *source,
791 const std::vector<std::string> &subspaces);
799 const StateSpace *sourceS,
const State *source,
800 const std::vector<std::string> &subspaces);
virtual bool isCompound(void) const
Check if the state space is compound.
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
void setName(const std::string &name)
Set the name of the state space.
bool isLocked(void) const
Return true if the state space is locked. A value of true means that no further spaces can be added a...
boost::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
int type_
A type assigned for this state space.
ParamSet params_
The set of parameters for this space.
virtual unsigned int validSegmentCount(const State *state1, const State *state2) const
Count how many segments of the "longest valid length" fit on the motion from state1 to state2...
Definition of a compound state.
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
static const std::string DEFAULT_PROJECTION_NAME
The name used for the default projection.
std::size_t index
The index of the value to be accessed, within the substate location above.
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
const std::vector< double > & getSubspaceWeights(void) const
Get the list of component weights.
bool includes(const StateSpacePtr &other) const
Return true if other is a space included (perhaps equal, perhaps a subspace) in this one...
static void Diagram(std::ostream &out)
Print a Graphviz digraph that represents the containment diagram for all the instantiated state space...
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings() ...
A boost shared pointer wrapper for ompl::base::StateSpace.
const T * as(void) const
Cast this instance to a desired type.
double getSubspaceWeight(const unsigned int index) const
Get the weight of a subspace from the compound state space (used in distance computation) ...
A boost shared pointer wrapper for ompl::base::StateSampler.
const ParamSet & params(void) const
Get the parameters for this space.
Check whether the StateSpace::distance() is bounded by StateSpace::getExtent()
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
std::vector< std::size_t > chain
In a complex state space there may be multiple compound state spaces that make up an even larger comp...
virtual unsigned int getDimension(void) const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual bool isHybrid(void) const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
Check whether sampled states are always within bounds.
Representation of the address of a value in a state. This structure stores the indexing information n...
virtual bool isHybrid(void) const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
virtual State * allocState(void) const
Allocate a state that can store a point in the described space.
AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
virtual double getLongestValidSegmentFraction(void) const
When performing discrete validation of motions, the length of the longest segment that does not requi...
const std::map< std::string, ValueLocation > & getValueLocationsByName(void) const
Get the named locations of values of type double contained in a state from this space. The setup() function must have been previously called.
CompoundState StateType
Define the type of state allocated by this state space.
virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
Register a projection for this state space under a specified name.
void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation() ...
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the state space. The first element of the signature is the number of integers that follow.
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
virtual StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
unsigned int longestValidSegmentCountFactor_
The factor to multiply the value returned by validSegmentCount()
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
StateSamplerAllocator ssa_
An optional state sampler allocator.
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
unsigned int componentCount_
The number of components.
Maintain a set of parameters.
const StateSpacePtr & getSubspace(const unsigned int index) const
Get a specific subspace from the compound state space.
State StateType
Define the type of state allocated by this space.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
const std::vector< ValueLocation > & getValueLocations(void) const
Get the locations of values of type double contained in a state from this space. The order of the val...
Check whether the distances between non-equal states is strictly positive (StateSpace::distance()) ...
virtual bool hasSymmetricInterpolate(void) const
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from, to, t, state) = interpolate(to, from, 1-t, state). Default implementation returns true.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
StateSpace(void)
Constructor. Assigns a unique name to the space.
A space to allow the composition of state spaces.
Representation of the address of a substate in a state. This structure stores the indexing informatio...
std::vector< double > weights_
The weight assigned to each component of the state space when computing the compound distance...
const std::string & getName(void) const
Get the name of the state space.
virtual double getMaximumExtent(void) const =0
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
unsigned int getSubspaceCount(void) const
Get the number of state spaces that make up the compound state space.
void setSubspaceWeight(const unsigned int index, double weight)
Set the weight of a subspace in the compound state space (used in distance computation) ...
virtual double getMaximumExtent(void) const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
Construct a compound state space that contains subspaces only from a. If a is compound, b (or the components from b, if b is compound) are removed and the remaining components are returned as a compound state space. If the compound space would end up containing solely one component, that component is returned instead.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
ParamSet & params(void)
Get the parameters for this space.
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
ProjectionEvaluatorPtr getDefaultProjection(void) const
Get the default projection.
virtual void freeState(State *state) const
Free the memory of the allocated state.
Definition of an abstract state.
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
Check whether the triangle inequality holds when using StateSpace::interpolate() and StateSpace::dist...
virtual bool satisfiesBounds(const State *state) const =0
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
void setValidSegmentCountFactor(unsigned int factor)
Set factor to be the value to multiply the return value of validSegmentCount(). By default...
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
CompoundStateSpace(void)
Construct an empty compound state space.
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
void lock(void)
Lock this state space. This means no further spaces can be added as components. This function can be ...
void setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections(void) const
Get all the registered projections.
bool covers(const StateSpacePtr &other) const
Return true if other is a space that is either included (perhaps equal, perhaps a subspace) in this o...
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
std::vector< ValueLocation > valueLocationsInOrder_
The value locations for all varliables of type double contained in a state; The locations point to va...
double maxExtent_
The extent of this space at the time setup() was called.
SanityChecks
Flags to use in a bit mask for state space sanity checks. Some basic checks do not have flags associa...
std::map< std::string, ProjectionEvaluatorPtr > projections_
List of available projections.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
virtual State * allocState(void) const =0
Allocate a state that can store a point in the described space.
virtual StateSamplerPtr allocStateSampler(void) const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
virtual void enforceBounds(State *state) const =0
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
virtual bool isDiscrete(void) const
Check if the set of states is discrete.
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
void getCommonSubspaces(const StateSpacePtr &other, std::vector< std::string > &subspaces) const
Get the set of subspaces that this space and other have in common. The computed list of subspaces doe...
double weightSum_
The sum of all the weights in weights_.
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
virtual bool hasSymmetricDistance(void) const
Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
SubstateLocation stateLocation
Location of the substate that contains the pointed to value.
virtual unsigned int validSegmentCount(const State *state1, const State *state2) const
Count how many segments of the "longest valid length" fit on the motion from state1 to state2...
virtual void sanityChecks(void) const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Check whether the distance function is symmetric (StateSpace::distance())
double longestValidSegmentFraction_
The fraction of the longest valid segment.
virtual bool equalStates(const State *state1, const State *state2) const =0
Checks whether two states are equal.
virtual double distance(const State *state1, const State *state2) const =0
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
bool locked_
Flag indicating whether adding further components is allowed or not.
unsigned int getValidSegmentCountFactor(void) const
Get the value used to multiply the return value of validSegmentCount().
bool hasProjection(const std::string &name) const
Check if a projection with a specified name is available.
static void List(std::ostream &out)
Print the list of available state space instances.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
std::map< std::string, SubstateLocation > substateLocationsByName_
All the known substat locations, by name.
virtual bool isCompound(void) const
Check if the state space is compound.
Check whether the StateSpace::serialize() and StateSpace::deserialize() work as expected.
Check whether calling StateSpace::interpolate() works as expected.
Check that enforceBounds() does not modify the contents of states that are within bounds...
double longestValidSegment_
The longest valid segment at the time setup() was called.
virtual void computeLocations(void)
Compute the location information for various components of the state space. Either this function or s...
bool hasDefaultProjection(void) const
Check if a default projection is available.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const =0
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
virtual void enforceBounds(State *state) const
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
double * getValueAddressAtLocation(State *state, const ValueLocation &loc) const
Get a pointer to the double value in state that loc points to.
double * getValueAddressAtName(State *state, const std::string &name) const
Get a pointer to the double value in state that name points to.
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual bool isMetricSpace(void) const
Return true if the distance function associated with the space is a metric.
virtual unsigned int getDimension(void) const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
void clearStateSamplerAllocator(void)
Clear the state sampler allocator (reset to default)
const StateSpace * space
The space that is reached if the chain above is followed on the state space.
AdvancedStateCopyOperation
The possible outputs for an advanced copy operation.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
const std::vector< StateSpacePtr > & getSubspaces(void) const
Get the list of components.
void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation() ...
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
T * as(void)
Cast this instance to a desired type.
virtual StateSamplerPtr allocDefaultStateSampler(void) const =0
Allocate an instance of the default uniform state sampler for this space.
virtual void computeLocations(void)
Compute the location information for various components of the state space. Either this function or s...
std::map< std::string, ValueLocation > valueLocationsByName_
All the known value locations, by name. The names of state spaces access the first element of a state...
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
int getType(void) const
Get the type of the state space. The type can be used to verify whether two space instances are of th...
T * as(const std::string &name) const
Cast a component of this instance to a desired type.
OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
const std::map< std::string, SubstateLocation > & getSubstateLocationsByName(void) const
Get the list of known substate locations (keys of the map corrspond to names of subspaces) ...
virtual bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
void list(std::ostream &out) const
Print the list of all contained state space instances.