ompl::control::OpenDEStateSpace Class Reference

State space representing OpenDE states. More...

#include <ompl/extensions/ode/OpenDEStateSpace.h>

Inheritance diagram for ompl::control::OpenDEStateSpace:

Classes

class  StateType
 OpenDE State. This is a compound state that allows accessing the properties of the bodies the state space is constructed for. More...
 

Public Types

enum  { STATE_COLLISION_KNOWN_BIT = 0, STATE_COLLISION_VALUE_BIT = 1, STATE_VALIDITY_KNOWN_BIT = 2, STATE_VALIDITY_VALUE_BIT = 3 }
 
- Public Types inherited from ompl::base::CompoundStateSpace
using StateType = ompl::base::CompoundState
 Define the type of state allocated by this state space.
 
- Public Types inherited from ompl::base::StateSpace
enum  SanityChecks {
  STATESPACE_DISTANCE_DIFFERENT_STATES = (1 << 1), STATESPACE_DISTANCE_SYMMETRIC = (1 << 2), STATESPACE_INTERPOLATION = (1 << 3), STATESPACE_TRIANGLE_INEQUALITY = (1 << 4),
  STATESPACE_DISTANCE_BOUND = (1 << 5), STATESPACE_RESPECT_BOUNDS = (1 << 6), STATESPACE_ENFORCE_BOUNDS_NO_OP = (1 << 7), STATESPACE_SERIALIZATION = (1 << 8)
}
 Flags to use in a bit mask for state space sanity checks. Some basic checks do not have flags associated (they are always executed; for example, whether copyState() works as expected) More...
 
using StateType = ompl::base::State
 Define the type of state allocated by this space.
 

Public Member Functions

 OpenDEStateSpace (OpenDEEnvironmentPtr env, double positionWeight=1.0, double linVelWeight=0.5, double angVelWeight=0.5, double orientationWeight=1.0)
 Construct a state space representing OpenDE states. More...
 
const OpenDEEnvironmentPtrgetEnvironment () const
 Get the OpenDE environment this state space corresponds to.
 
unsigned int getNrBodies () const
 Get the number of bodies state is maintained for.
 
void setDefaultBounds ()
 By default, the volume bounds enclosing the geometry of the environment are computed to include all objects in the spaces collision checking is performed (env.collisionSpaces_). The linear and angular velocity bounds are set as -1 to 1 for each dimension.
 
void setVolumeBounds (const base::RealVectorBounds &bounds)
 Set the bounds for each of the position subspaces.
 
void setLinearVelocityBounds (const base::RealVectorBounds &bounds)
 Set the bounds for each of the linear velocity subspaces.
 
void setAngularVelocityBounds (const base::RealVectorBounds &bounds)
 Set the bounds for each of the angular velocity subspaces.
 
virtual void readState (base::State *state) const
 Read the parameters of the OpenDE bodies and store them in state.
 
virtual void writeState (const base::State *state) const
 Set the parameters of the OpenDE bodies to be the ones read from state. The code will technically work if this function is called from multiple threads simultaneously, but the results are unpredictable.
 
bool satisfiesBoundsExceptRotation (const StateType *state) const
 This is a convenience function provided for optimization purposes. It checks whether a state satisfies its bounds. Typically, in the process of simulation the rotations remain valid (very slightly out of bounds), so there is no point in updating or checking them. This function checks all other bounds (position, linear and agular velocities)
 
base::StateallocState () const override
 Allocate a state that can store a point in the described space.
 
void freeState (base::State *state) const override
 Free the memory of the allocated state.
 
void copyState (base::State *destination, const base::State *source) const override
 Copy a state to another. The memory of source and destination should NOT overlap. More...
 
void interpolate (const base::State *from, const base::State *to, double t, base::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. The memory location of state is not required to be different from the memory of either from or to.
 
base::StateSamplerPtr allocDefaultStateSampler () const override
 Allocate an instance of the default uniform state sampler for this space.
 
base::StateSamplerPtr allocStateSampler () const override
 Allocate an instance of the state sampler for this space. This sampler will be allocated with the sampler allocator that was previously specified by setStateSamplerAllocator() or, if no sampler allocator was specified, allocDefaultStateSampler() is called.
 
virtual bool evaluateCollision (const base::State *state) const
 Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state, if unspecified. Return the value value of that bit.
 
- Public Member Functions inherited from ompl::base::CompoundStateSpace
 CompoundStateSpace ()
 Construct an empty compound state space.
 
 CompoundStateSpace (const std::vector< StateSpacePtr > &components, const std::vector< double > &weights)
 Construct a compound state space from a list of subspaces (components) and their corresponding weights (weights)
 
template<class T >
T * as (const unsigned int index) const
 Cast a component of this instance to a desired type. More...
 
template<class T >
T * as (const std::string &name) const
 Cast a component of this instance to a desired type. More...
 
bool isCompound () const override
 Check if the state space is compound.
 
bool isHybrid () const override
 Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
 
void printState (const State *state, std::ostream &out) const override
 Print a state to a stream.
 
void printSettings (std::ostream &out) const override
 Print the settings for this state space to a stream.
 
void computeLocations () override
 Compute the location information for various components of the state space. Either this function or setup() must be called before any calls to getValueAddressAtName(), getValueAddressAtLocation() (and other functions where those are used).
 
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.
 
void addSubspace (const StateSpacePtr &component, double weight)
 Adds a new state space as part of the compound state space. For computing distances within the compound state space, the weight of the component also needs to be specified.
 
unsigned int getSubspaceCount () const
 Get the number of state spaces that make up the compound state space.
 
const StateSpacePtrgetSubspace (unsigned int index) const
 Get a specific subspace from the compound state space.
 
const StateSpacePtrgetSubspace (const std::string &name) const
 Get a specific subspace from the compound state space.
 
unsigned int getSubspaceIndex (const std::string &name) const
 Get the index of a specific subspace from the compound state space.
 
bool hasSubspace (const std::string &name) const
 Check if a specific subspace is contained in this state space.
 
double getSubspaceWeight (unsigned int index) const
 Get the weight of a subspace from the compound state space (used in distance computation)
 
double getSubspaceWeight (const std::string &name) const
 Get the weight of a subspace from the compound state space (used in distance computation)
 
void setSubspaceWeight (unsigned int index, double weight)
 Set the weight of a subspace in the compound state space (used in distance computation)
 
void setSubspaceWeight (const std::string &name, double weight)
 Set the weight of a subspace in the compound state space (used in distance computation)
 
const std::vector< StateSpacePtr > & getSubspaces () const
 Get the list of components.
 
const std::vector< double > & getSubspaceWeights () const
 Get the list of component weights.
 
bool isLocked () const
 Return true if the state space is locked. A value of true means that no further spaces can be added as components.
 
void lock ()
 Lock this state space. This means no further spaces can be added as components. This function can be for instance called from the constructor of a state space that inherits from CompoundStateSpace to prevent the user to add further components.
 
StateSamplerPtr allocSubspaceStateSampler (const StateSpace *subspace) const override
 Allocate a sampler that actually samples only components that are part of subspace.
 
unsigned int getDimension () const override
 Get the dimension of the space (not the dimension of the surrounding ambient space)
 
double getMaximumExtent () const override
 Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces, this function can return infinity. More...
 
double getMeasure () const override
 Get a measure of the space (this can be thought of as a generalization of volume)
 
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-op.
 
bool satisfiesBounds (const State *state) const override
 Check if a state is inside the bounding box. For unbounded spaces this function can always return true.
 
unsigned int getSerializationLength () const override
 Get the number of chars in the serialization of a state in this space.
 
void serialize (void *serialization, const State *state) const override
 Write the binary representation of state to serialization.
 
void deserialize (State *state, const void *serialization) const override
 Read the binary representation of a state from serialization and write it to state.
 
double distance (const State *state1, const State *state2) const override
 Computes distance between two states. This function satisfies the properties of a metric if isMetricSpace() is true, and its return value will always be between 0 and getMaximumExtent()
 
void setLongestValidSegmentFraction (double segmentFraction) override
 When performing discrete validation of motions, the length of the longest segment that does not require state validation needs to be specified. This function sets this length as a fraction of the space's maximum extent. The call is passed to all contained subspaces.
 
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. This is the max() of the counts returned by contained subspaces.
 
bool equalStates (const State *state1, const State *state2) const override
 Checks whether two states are equal.
 
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 address of a double value from state state located at position index. The first double value is returned for index = 0. If index is too large (does not point to any double values in the state), the return value is nullptr. More...
 
- Public Member Functions inherited from ompl::base::StateSpace
 StateSpace (const StateSpace &)=delete
 
StateSpaceoperator= (const StateSpace &)=delete
 
 StateSpace ()
 Constructor. Assigns a unique name to the space.
 
template<class T >
T * as ()
 Cast this instance to a desired type. More...
 
template<class T >
const T * as () const
 Cast this instance to a desired type. More...
 
virtual bool isDiscrete () const
 Check if the set of states is discrete. More...
 
virtual bool isMetricSpace () const
 Return true if the distance function associated with the space is a metric.
 
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.
 
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.
 
const std::string & getName () const
 Get the name of the state space.
 
void setName (const std::string &name)
 Set the name of the state space.
 
int getType () const
 Get the type of the state space. The type can be used to verify whether two space instances are of the same type (e.g., SO2)
 
bool includes (const StateSpacePtr &other) const
 Return true if other is a space included (perhaps equal, perhaps a subspace) in this one.
 
bool includes (const StateSpace *other) const
 Return true if other is a space included (perhaps equal, perhaps a subspace) in this one.
 
bool covers (const StateSpacePtr &other) const
 Return true if other is a space that is either included (perhaps equal, perhaps a subspace) in this one, or all of its subspaces are included in this one.
 
bool covers (const StateSpace *other) const
 Return true if other is a space that is either included (perhaps equal, perhaps a subspace) in this one, or all of its subspaces are included in this one.
 
ParamSetparams ()
 Get the parameters for this space.
 
const ParamSetparams () const
 Get the parameters for this space.
 
virtual double getLongestValidSegmentFraction () const
 When performing discrete validation of motions, the length of the longest segment that does not require state validation needs to be specified. This function returns this length, for this state space, as a fraction of the space's maximum extent.
 
virtual void setValidSegmentCountFactor (unsigned int factor)
 Set factor to be the value to multiply the return value of validSegmentCount(). By default, this value is 1. The higher the value, the smaller the size of the segments considered valid. The effect of this function is immediate (setup() does not need to be called).
 
virtual unsigned int getValidSegmentCountFactor () const
 Get the value used to multiply the return value of validSegmentCount().
 
virtual double getLongestValidSegmentLength () const
 Get the longest valid segment at the time setup() was called.
 
virtual 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.
 
StatecloneState (const State *source) const
 Clone a state.
 
void setStateSamplerAllocator (const StateSamplerAllocator &ssa)
 Set the sampler allocator to use.
 
void clearStateSamplerAllocator ()
 Clear the state sampler allocator (reset to default)
 
const double * getValueAddressAtIndex (const State *state, unsigned int index) const
 Const variant of the same function as above;.
 
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 values is consistent with getValueAddressAtIndex(). The setup() function must have been previously called.
 
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.
 
double * getValueAddressAtLocation (State *state, const ValueLocation &loc) const
 Get a pointer to the double value in state that loc points to.
 
const double * getValueAddressAtLocation (const State *state, const ValueLocation &loc) const
 Const variant of the same function as above;.
 
double * getValueAddressAtName (State *state, const std::string &name) const
 Get a pointer to the double value in state that name points to.
 
const double * getValueAddressAtName (const State *state, const std::string &name) const
 Const variant of the same function as above;.
 
virtual 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()
 
virtual void copyFromReals (State *destination, const std::vector< double > &reals) const
 Copy the values from reals to the state destination using getValueAddressAtLocation()
 
void registerProjection (const std::string &name, const ProjectionEvaluatorPtr &projection)
 Register a projection for this state space under a specified name.
 
void registerDefaultProjection (const ProjectionEvaluatorPtr &projection)
 Register the default projection for this state space.
 
virtual void registerProjections ()
 Register the projections for this state space. Usually, this is at least the default projection. These are implicit projections, set by the implementation of the state space. This is called by setup().
 
ProjectionEvaluatorPtr getProjection (const std::string &name) const
 Get the projection registered under a specific name.
 
ProjectionEvaluatorPtr getDefaultProjection () const
 Get the default projection.
 
bool hasProjection (const std::string &name) const
 Check if a projection with a specified name is available.
 
bool hasDefaultProjection () const
 Check if a default projection is available.
 
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections () const
 Get all the registered projections.
 
StateSamplerPtr allocSubspaceStateSampler (const StateSpacePtr &subspace) const
 Allocate a sampler that actually samples only components that are part of subspace.
 
StategetSubstateAtLocation (State *state, const SubstateLocation &loc) const
 Get the substate of state that is pointed to by loc.
 
const StategetSubstateAtLocation (const State *state, const SubstateLocation &loc) const
 Get the substate of state that is pointed to by loc.
 
const std::map< std::string, SubstateLocation > & getSubstateLocationsByName () const
 Get the list of known substate locations (keys of the map corrspond to names of subspaces)
 
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 does not contain spaces that cover each other, even though they may be common, as that is redundant information.
 
void getCommonSubspaces (const StateSpace *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 does not contain spaces that cover each other, even though they may be common, as that is redundant information.
 
virtual void printProjections (std::ostream &out) const
 Print the list of registered projections. This function is also called by printSettings()
 
virtual void sanityChecks (double zero, double eps, unsigned int flags) const
 Perform sanity checks for this state space. Throws an exception if failures are found. More...
 
virtual void sanityChecks () const
 Convenience function that allows derived state spaces to choose which checks should pass (see SanityChecks flags) and how strict the checks are. This just calls sanityChecks() with some default arguments.
 
void diagram (std::ostream &out) const
 Print a Graphviz digraph that represents the containment diagram for the state space.
 
void list (std::ostream &out) const
 Print the list of all contained state space instances.
 

Protected Attributes

OpenDEEnvironmentPtr env_
 Representation of the OpenDE parameters OMPL needs to plan.
 
- Protected Attributes inherited from ompl::base::CompoundStateSpace
std::vector< StateSpacePtrcomponents_
 The state spaces that make up the compound state space.
 
unsigned int componentCount_ {0u}
 The number of components.
 
std::vector< double > weights_
 The weight assigned to each component of the state space when computing the compound distance.
 
double weightSum_ {0.0}
 The sum of all the weights in weights_.
 
bool locked_ {false}
 Flag indicating whether adding further components is allowed or not.
 
- Protected Attributes inherited from ompl::base::StateSpace
int type_
 A type assigned for this state space.
 
StateSamplerAllocator ssa_
 An optional state sampler allocator.
 
double maxExtent_
 The extent of this space at the time setup() was called.
 
double longestValidSegmentFraction_
 The fraction of the longest valid segment.
 
double longestValidSegment_
 The longest valid segment at the time setup() was called.
 
unsigned int longestValidSegmentCountFactor_
 The factor to multiply the value returned by validSegmentCount(). Rarely used but useful for things like doubling the resolution.
 
std::map< std::string, ProjectionEvaluatorPtrprojections_
 List of available projections.
 
ParamSet params_
 The set of parameters for this space.
 
std::vector< ValueLocationvalueLocationsInOrder_
 The value locations for all varliables of type double contained in a state; The locations point to values in the same order as that returned by getValueAddressAtIndex()
 
std::map< std::string, ValueLocationvalueLocationsByName_
 All the known value locations, by name. The names of state spaces access the first element of a state. RealVectorStateSpace dimensions are used to access individual dimensions.
 
std::map< std::string, SubstateLocationsubstateLocationsByName_
 All the known substat locations, by name.
 

Additional Inherited Members

- Static Public Member Functions inherited from ompl::base::StateSpace
static void Diagram (std::ostream &out)
 Print a Graphviz digraph that represents the containment diagram for all the instantiated state spaces.
 
static void List (std::ostream &out)
 Print the list of available state space instances.
 
- Protected Member Functions inherited from ompl::base::CompoundStateSpace
void allocStateComponents (CompoundState *state) const
 Allocate the state components. Called by allocState(). Usually called by derived state spaces.
 
- Static Protected Attributes inherited from ompl::base::StateSpace
static const std::string DEFAULT_PROJECTION_NAME = ""
 The name used for the default projection.
 

Detailed Description

State space representing OpenDE states.

Definition at line 114 of file OpenDEStateSpace.h.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum
Enumerator
STATE_COLLISION_KNOWN_BIT 

Index of bit in StateType::collision indicating whether it is known if a state is in collision or not. Initially this is 0. The value of this bit is updated by OpenDEStateSpace::evaluateCollision() and OpenDEControlSpace::propagate().

STATE_COLLISION_VALUE_BIT 

Index of bit in StateType::collision indicating whether a state is in collision or not. Initially the value of this field is unspecified. The value gains meaning (1 or 0) when OpenDEStateSpace::STATE_COLLISION_KNOWN_BIT becomes 1. The value of this bit is updated by OpenDEStateSpace::evaluateCollision() and OpenDEControlSpace::propagate(). A value of 1 implies that there is no collision for which OpenDEEnvironment::isValidCollision() returns false.

STATE_VALIDITY_KNOWN_BIT 

Index of bit in StateType::collision indicating whether it is known if a state is in valid or not. Initially this is 0. The value of this bit is updated by OpenDEStateValidityChecker::isValid(). This bit is only used if the OpenDEStateValidityChecker is used.

STATE_VALIDITY_VALUE_BIT 

Index of bit in StateType::collision indicating whether a state is valid or not. Initially the value of this field is unspecified. The value gains meaning (1 or 0) when OpenDEStateSpace::STATE_VALIDITY_KNOWN_BIT becomes 1. The value of this bit is updated by OpenDEEnvironment::isValid(). A value of 1 implies that a state is valid. This bit is only used if the OpenDEStateValidityChecker is used.

Definition at line 149 of file OpenDEStateSpace.h.

Constructor & Destructor Documentation

◆ OpenDEStateSpace()

ompl::control::OpenDEStateSpace::OpenDEStateSpace ( OpenDEEnvironmentPtr  env,
double  positionWeight = 1.0,
double  linVelWeight = 0.5,
double  angVelWeight = 0.5,
double  orientationWeight = 1.0 
)

Construct a state space representing OpenDE states.

This will be a compound state space with 4 components for each body in env.stateBodies_. The 4 subspaces constructed for each body are: position (R3), linear velocity (R3), angular velocity (R3) and orientation (SO(3)). Default bounds are set by calling setDefaultBounds().

Parameters
envthe environment to construct the state space for
positionWeightthe weight to pass to CompoundStateSpace::addSubspace() for position subspaces
linVelWeightthe weight to pass to CompoundStateSpace::addSubspace() for linear velocity subspaces
angVelWeightthe weight to pass to CompoundStateSpace::addSubspace() for angular velocity subspaces
orientationWeightthe weight to pass to CompoundStateSpace::addSubspace() for orientation subspaces

Definition at line 43 of file OpenDEStateSpace.cpp.

Member Function Documentation

◆ copyState()

void ompl::control::OpenDEStateSpace::copyState ( base::State destination,
const base::State source 
) const
overridevirtual

Copy a state to another. The memory of source and destination should NOT overlap.

Note
For more advanced state copying methods (partial copy, for example), see Advanced methods for copying states.

Reimplemented from ompl::base::CompoundStateSpace.

Definition at line 156 of file OpenDEStateSpace.cpp.


The documentation for this class was generated from the following files: