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();
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
Definition: StateSpace.cpp:730
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections() const
Get all the registered projections.
Definition: StateSpace.cpp:754
double maxExtent_
The extent of this space at the time setup() was called.
Definition: StateSpace.h:601
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
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: StateSpace.cpp:1181
void clearStateSamplerAllocator()
Clear the state sampler allocator (reset to default)
Definition: StateSpace.cpp:802
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:165
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:382
double longestValidSegment_
The longest valid segment at the time setup() was called.
Definition: StateSpace.h:607
virtual double getLongestValidSegmentFraction() const
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:848
A space to allow the composition of state spaces.
Definition: StateSpace.h:637
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
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: StateSpace.cpp:1022
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: StateSpace.cpp:1117
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
Definition: StateSpace.cpp:1212
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...
Definition: StateSpace.cpp:1244
void setValidSegmentCountFactor(unsigned int factor)
Set factor to be the value to multiply the return value of validSegmentCount(). By default,...
Definition: StateSpace.cpp:827
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:178
void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
Register a projection for this state space under a specified name.
Definition: StateSpace.cpp:764
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
Definition: StateSpace.cpp:1126
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
Definition: StateSpace.cpp:300
Representation of the address of a value in a state. This structure stores the indexing information n...
Definition: StateSpace.h:186
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
SubstateLocation stateLocation
Location of the substate that contains the pointed to value.
Definition: StateSpace.h:189
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound state space.
Definition: StateSpace.cpp:917
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
Definition: StateSpace.cpp:1233
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition: StateSpace.cpp:610
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
Definition: StateSpace.cpp:1171
unsigned int getValidSegmentCountFactor() const
Get the value used to multiply the return value of validSegmentCount().
Definition: StateSpace.cpp:843
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
Definition: StateSpace.cpp:807
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
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:912
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
Definition: StateSpace.cpp:996
const std::vector< double > & getSubspaceWeights() const
Get the list of component weights.
Definition: StateSpace.cpp:991
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap.
double getSubspaceWeight(unsigned int index) const
Get the weight of a subspace from the compound state space (used in distance computation)
Definition: StateSpace.cpp:946
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: StateSpace.cpp:1076
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: StateSpace.cpp:1029
virtual bool isHybrid() const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
Definition: StateSpace.cpp:782
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
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
Definition: StateSpace.cpp:1013
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 deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
Definition: StateSpace.cpp:1065
bool isLocked() const
Return true if the state space is locked. A value of true means that no further spaces can be added a...
Definition: StateSpace.cpp:1176
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
double getLongestValidSegmentLength() const
Get the longest valid segment at the time setup() was called.
Definition: StateSpace.cpp:853
void setSubspaceWeight(unsigned int index, double weight)
Set the weight of a subspace in the compound state space (used in distance computation)
Definition: StateSpace.cpp:962
double longestValidSegmentFraction_
The fraction of the longest valid segment.
Definition: StateSpace.h:604
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
Definition: StateSpace.cpp:815
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
Definition: StateSpace.cpp:1107
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
Definition: StateSpace.cpp:1203
static const std::string DEFAULT_PROJECTION_NAME
The name used for the default projection.
Definition: StateSpace.h:592
void freeState(State *state) const override
Free the memory of the allocated state.
Definition: StateSpace.cpp:1162
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:759
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
virtual bool isDiscrete() const
Check if the set of states is discrete.
Definition: StateSpace.cpp:777
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection....
Definition: StateSpace.cpp:244
StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
Construct a compound state space that contains subspaces only from a. If a is compound,...
Definition: StateSpace.cpp:1461
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
Definition: StateSpace.cpp:1138
Representation of the address of a substate in a state. This structure stores the indexing informatio...
Definition: StateSpace.h:171
bool hasProjection(const std::string &name) const
Check if a projection with a specified name is available.
Definition: StateSpace.cpp:725
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
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:835
State * allocState() const override
Allocate a state that can store a point in the described space.
Definition: StateSpace.cpp:1148
const std::map< std::string, ValueLocation > & getValueLocationsByName() const
Get the named locations of values of type double contained in a state from this space....
Definition: StateSpace.cpp:335
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Definition: StateSpace.cpp:1004
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Definition: StateSpace.cpp:1038
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:255
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
Definition: StateSpace.cpp:400
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:1046
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: StateSpace.cpp:1093
virtual void computeLocations()
Compute the location information for various components of the state space. Either this function or s...
Definition: StateSpace.cpp:225
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
Definition: StateSpace.cpp:925
bool hasDefaultProjection() const
Check if a default projection is available.
Definition: StateSpace.cpp:720
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 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:296
AdvancedStateCopyOperation
The possible outputs for an advanced copy operation.
Definition: StateSpace.h:850
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the state space....
Definition: StateSpace.cpp:230
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
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
Definition: StateSpace.cpp:1155
std::size_t index
The index of the value to be accessed, within the substate location above.
Definition: StateSpace.h:192
unsigned int longestValidSegmentCountFactor_
The factor to multiply the value returned by validSegmentCount(). Rarely used but useful for things l...
Definition: StateSpace.h:611
const StateSpace * space
The space that is reached if the chain above is followed on the state space.
Definition: StateSpace.h:181
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
Definition: StateSpace.cpp:560
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:1086
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
Definition: OptimizationObjective.cpp:268
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
virtual double getMeasure() const =0
Get a measure of the space (this can be thought of as a generalization of volume)
A shared pointer wrapper for ompl::base::StateSpace.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition: StateSpace.cpp:986
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 setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
Definition: StateSpace.cpp:797
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
Definition: StateSpace.cpp:1054
virtual bool hasSymmetricDistance() const
Check if the distance function on this state space is symmetric, i.e. distance(s1,...
Definition: StateSpace.cpp:787
A shared pointer wrapper for ompl::base::StateSampler.
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings()
Definition: StateSpace.cpp:406
void list(std::ostream &out) const
Print the list of all contained state space instances.
Definition: StateSpace.cpp:542
static void Diagram(std::ostream &out)
Print a Graphviz digraph that represents the containment diagram for all the instantiated state space...
Definition: StateSpace.cpp:587
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...
Definition: StateSpace.cpp:879
static void List(std::ostream &out)
Print the list of available state space instances.
Definition: StateSpace.cpp:534
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation....
Definition: StateSpace.cpp:248
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
Definition: StateSpace.cpp:741
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
Definition: StateSpace.cpp:896
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
Definition: StateSpace.cpp:1225
virtual bool hasSymmetricInterpolate() const
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from,...
Definition: StateSpace.cpp:792
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
Definition: StateSpace.cpp:933
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: StateSpace.cpp:858
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
Definition: StateSpace.cpp:387
virtual void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
Definition: StateSpace.cpp:395
OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
Definition: OptimizationObjective.cpp:306