StateSpace.cpp
133 static void computeStateSpaceSignatureHelper(const StateSpace *space, std::vector<int> &signature)
142 computeStateSpaceSignatureHelper(space->as<CompoundStateSpace>()->getSubspace(i).get(), signature);
201 computeLocationsHelper(s, substateMap, locationsArray, locationsMap, StateSpace::ValueLocation());
216 // we don't want to call this function during the state space construction because calls to virtual functions are
222 computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
227 computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
256 error << "The longest valid segment for state space " + getName() + " must be positive." << std::endl;
262 computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
300 ompl::base::State *ompl::base::StateSpace::getSubstateAtLocation(State *state, const SubstateLocation &loc) const
317 double *ompl::base::StateSpace::getValueAddressAtIndex(State * /*state*/, const unsigned int /*index*/) const
322 const double *ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
329 const std::vector<ompl::base::StateSpace::ValueLocation> &ompl::base::StateSpace::getValueLocations() const
347 void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
354 double *ompl::base::StateSpace::getValueAddressAtLocation(State *state, const ValueLocation &loc) const
362 const double *ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
370 double *ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
373 return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
376 const double *ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
379 return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
391 void ompl::base::StateSpace::deserialize(State * /*state*/, const void * /*serialization*/) const
468 bool operator()(const StateSpace::SubstateLocation &a, const StateSpace::SubstateLocation &b) const
500 void ompl::base::StateSpace::getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const
505 void ompl::base::StateSpace::getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const
508 const std::map<std::string, StateSpace::SubstateLocation> &S = other->getSubstateLocationsByName();
612 unsigned int flags = isMetricSpace() ? ~0 : ~(STATESPACE_DISTANCE_SYMMETRIC | STATESPACE_TRIANGLE_INEQUALITY);
613 sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), flags);
638 throw Exception("Copy of a state is not the same as the original state. copyState() may not work "
667 throw Exception("The distance function should not report values larger than the maximum extent "
694 throw Exception("Interpolation from a state at time 0 should be not change the original state");
703 throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle "
741 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getProjection(const std::string &name) const
759 void ompl::base::StateSpace::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
764 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
769 OMPL_ERROR("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
815 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocSubspaceStateSampler(const StateSpacePtr &subspace) const
820 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocSubspaceStateSampler(const StateSpace *subspace) const
830 throw Exception("The multiplicative factor for the valid segment count between two states must be strictly "
858 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
860 return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
868 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
917 const ompl::base::StateSpacePtr &ompl::base::CompoundStateSpace::getSubspace(const unsigned int index) const
941 const ompl::base::StateSpacePtr &ompl::base::CompoundStateSpace::getSubspace(const std::string &name) const
986 const std::vector<ompl::base::StateSpacePtr> &ompl::base::CompoundStateSpace::getSubspaces() const
1008 if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
1017 if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
1065 void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
1071 components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char *>(serialization) + l);
1076 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
1082 dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
1093 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
1100 unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
1107 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
1117 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
1123 components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
1138 ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocSubspaceStateSampler(const StateSpace *subspace) const
1181 double *ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
1244 AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
1250 AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
1294 // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
1295 // it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed
1306 AdvancedStateCopyOperation res = copyStateData(destS, dest, compoundSourceS->getSubspace(i).get(),
1322 AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
1328 AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
1332 const std::map<std::string, StateSpace::SubstateLocation> &destLoc = destS->getSubstateLocationsByName();
double * getValueAddressAtName(State *state, const std::string &name) const
Get a pointer to the double value in state that name points to.
Definition: StateSpace.cpp:370
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:212
State * allocState() const override
Allocate a state that can store a point in the described space.
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
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...
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume) ...
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.
Definition: StateSpace.cpp:230
static const std::string DEFAULT_PROJECTION_NAME
The name used for the default projection.
Definition: StateSpace.h:528
std::size_t index
The index of the value to be accessed, within the substate location above.
Definition: StateSpace.h:128
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections() const
Get all the registered projections.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
bool hasDefaultProjection() const
Check if a default projection is available.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
A shared pointer wrapper for ompl::base::StateSpace.
OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
Definition: OptimizationObjective.cpp:306
double * getValueAddressAtLocation(State *state, const ValueLocation &loc) const
Get a pointer to the double value in state that loc points to.
Definition: StateSpace.cpp:354
A shared pointer wrapper for ompl::base::StateSampler.
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
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...
Definition: StateSpace.h:114
void list(std::ostream &out) const
Print the list of all contained state space instances.
Definition: StateSpace.cpp:542
Representation of the address of a value in a state. This structure stores the indexing information n...
Definition: StateSpace.h:122
void setSubspaceWeight(unsigned int index, double weight)
Set the weight of a subspace in the compound state space (used in distance computation) ...
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...
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
Definition: StateSpace.cpp:387
bool includes(const StateSpacePtr &other) const
Return true if other is a space included (perhaps equal, perhaps a subspace) in this one...
Definition: StateSpace.cpp:485
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...
void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
Register a projection for this state space under a specified name.
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...
bool isCompound() const override
Check if the state space is compound.
virtual void computeLocations()
Compute the location information for various components of the state space. Either this function or s...
Definition: StateSpace.cpp:225
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:244
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(). Rarely used but useful for things l...
Definition: StateSpace.h:547
double getSubspaceWeight(unsigned int index) const
Get the weight of a subspace from the compound state space (used in distance computation) ...
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
const std::map< std::string, ValueLocation > & getValueLocationsByName() 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.
Definition: StateSpace.cpp:335
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
double getLongestValidSegmentLength() const
Get the longest valid segment at the time setup() was called.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
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...
Definition: StateSpace.cpp:480
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
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() ...
Definition: StateSpace.cpp:340
virtual bool hasSymmetricInterpolate() 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 deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
Definition: StateSpace.cpp:391
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-...
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...
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
Definition: StateSpace.cpp:400
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
virtual void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
Definition: StateSpace.cpp:395
A space to allow the composition of state spaces.
Definition: StateSpace.h:573
unsigned int getValidSegmentCountFactor() const
Get the value used to multiply the return value of validSegmentCount().
Representation of the address of a substate in a state. This structure stores the indexing informatio...
Definition: StateSpace.h:107
virtual bool isCompound() const
Check if the state space is compound.
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
virtual bool isDiscrete() const
Check if the set of states is discrete.
bool hasProjection(const std::string &name) const
Check if a projection with a specified name is available.
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:382
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
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.
Definition: StateSpace.h:70
virtual void setup()
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.
Definition: StateSpace.cpp:248
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound state space.
virtual double * getValueAddressAtIndex(State *state, unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
Definition: StateSpace.cpp:317
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...
Definition: OptimizationObjective.cpp:268
const std::map< std::string, SubstateLocation > & getSubstateLocationsByName() const
Get the list of known substate locations (keys of the map corrspond to names of subspaces) ...
Definition: StateSpace.cpp:295
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
Definition: StateSpace.cpp:300
void setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
virtual double getMeasure() const =0
Get a measure of the space (this can be thought of as a generalization of volume) ...
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...
double maxExtent_
The extent of this space at the time setup() was called.
Definition: StateSpace.h:537
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=typename SpecificParam< T >::GetterFn())
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:232
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
bool isLocked() const
Return true if the state space is locked. A value of true means that no further spaces can be added a...
void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation() ...
Definition: StateSpace.cpp:347
const std::vector< ValueLocation > & getValueLocations() const
Get the locations of values of type double contained in a state from this space. The order of the val...
Definition: StateSpace.cpp:329
ompl::base::RealVectorStateSpace
Definition: StateSpaceTypes.h:52
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
SubstateLocation stateLocation
Location of the substate that contains the pointed to value.
Definition: StateSpace.h:125
double longestValidSegmentFraction_
The fraction of the longest valid segment.
Definition: StateSpace.h:540
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings() ...
Definition: StateSpace.cpp:406
static void List(std::ostream &out)
Print the list of available state space instances.
Definition: StateSpace.cpp:534
virtual bool isHybrid() const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
virtual bool hasSymmetricDistance() const
Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
void freeState(State *state) const override
Free the memory of the allocated state.
const std::vector< double > & getSubspaceWeights() const
Get the list of component weights.
double longestValidSegment_
The longest valid segment at the time setup() was called.
Definition: StateSpace.h:543
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
CompoundStateSpace()
Construct an empty compound state space.
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
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:191
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
Definition: StateSpace.cpp:560
const StateSpace * space
The space that is reached if the chain above is followed on the state space.
Definition: StateSpace.h:117
AdvancedStateCopyOperation
The possible outputs for an advanced copy operation.
Definition: StateSpace.h:786
static void Diagram(std::ostream &out)
Print a Graphviz digraph that represents the containment diagram for all the instantiated 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...
Definition: StateSpace.cpp:500
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...
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...
void clearStateSamplerAllocator()
Clear the state sampler allocator (reset to default)
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
virtual double getLongestValidSegmentFraction() const
When performing discrete validation of motions, the length of the longest segment that does not requi...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
void setup() override
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.
Unset type; this is the default type.
Definition: StateSpaceTypes.h:49