ompl::base::ConstrainedStateSpace Class Referenceabstract

A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambient space, and the constraint is used to inform any manifold related operations. setSpaceInformation() must be called in order for collision checking to be done in tandem with manifold traversal. More...

#include <ompl/base/spaces/constraint/ConstrainedStateSpace.h>

Inheritance diagram for ompl::base::ConstrainedStateSpace:

Classes

class  StateType
 A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and efficiency of various Constraint related operations, this State inherits from Eigen::Map<Eigen::VectorXd>, mapping the underlying dense double vector into an Eigen::VectorXd. Note that this state type inherits from WrapperStateSpace::StateType, and as such the underlying state can be accessed by getState(). More...
 

Public Types

enum  SanityChecks {
  CONSTRAINED_STATESPACE_SAMPLERS = (1 << 1), CONSTRAINED_STATESPACE_GEODESIC_SATISFY = (1 << 2), CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY = (1 << 3), CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE = (1 << 4),
  CONSTRAINED_STATESPACE_JACOBIAN = (1 << 5)
}
 Flags used in a bit mask for constrained state space sanity checks, constraintSanityChecks(). More...
 
- 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

 ConstrainedStateSpace (const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
 Construct a constrained space from an ambientSpace and a constraint.
 
State Space Related Operations
bool isMetricSpace () const override
 Returns false as the implicit constrained configuration space defined by the constraint is not metric with respect to the ambient configuration space's metric.
 
void setSpaceInformation (SpaceInformation *si)
 Sets the space information for this state space. Required for collision checking in manifold traversal.
 
void setup () override
 Final setup for the space.
 
virtual void clear ()
 Clear any allocated memory from the state space.
 
StateallocState () const override
 Allocate a new state in this space.
 
void constrainedSanityChecks (unsigned int flags) const
 Do some sanity checks relating to discrete geodesic computation and constraint satisfaction. See SanityChecks flags.
 
void sanityChecks () const override
 Perform both constrained and regular sanity checks.
 
unsigned int validSegmentCount (const State *s1, const State *s2) const override
 Return the valid segment count on the manifold, as valid segment count is determined by delta_ and lambda_.
 
Constrained Planning
void interpolate (const State *from, const State *to, double t, State *state) const override
 Find a state between from and to around time t, where t = 0 is from, and t = 1 is the final state reached by discreteGeodesic(from, to, true, ...), which may not be to. State returned in state.
 
virtual bool discreteGeodesic (const State *from, const State *to, bool interpolate=false, std::vector< State * > *geodesic=nullptr) const =0
 Traverse the manifold from from toward to. Returns true if we reached to, and false if we stopped early for any reason, such as a collision or traveling too far. No collision checking is performed if interpolate is true. If geodesic is not nullptr, the sequence of intermediates is saved to it, including a copy of from, as well as the final state, which is a copy of to if we reached to. Caller is responsible for freeing states returned in geodesic. Needs to be implemented by any constrained state space.
 
virtual StategeodesicInterpolate (const std::vector< State * > &geodesic, double t) const
 Like interpolate(...), but interpolates between intermediate states already supplied in stateList from a previous call to discreteGeodesic(..., geodesic). The from and to states are the first and last elements stateList. Returns a pointer to a state in geodesic.
 
Setters and Getters
void setDelta (double delta)
 Set delta, the step size for traversing the manifold and collision checking. Default defined by ompl::magic::CONSTRAINED_STATE_SPACE_DELTA.
 
void setLambda (double lambda)
 Set lambda, where lambda * distance(x, y) is the maximum length of the geodesic x to y. Additionally, lambda * delta is the greatest distance a point can diverge from its previous step, to preserve continuity. Must be greater than 1.
 
double getDelta () const
 Get delta, the step size across the manifold.
 
double getLambda () const
 Get lambda (see setLambda()).
 
unsigned int getAmbientDimension () const
 Returns the dimension of the ambient space.
 
unsigned int getManifoldDimension () const
 Returns the dimension of the manifold.
 
const ConstraintPtr getConstraint () const
 Returns the constraint that defines the underlying manifold.
 
- Public Member Functions inherited from ompl::base::WrapperStateSpace
 WrapperStateSpace (const StateSpacePtr &space)
 
bool isCompound () const override
 Check if the state space is compound.
 
bool isDiscrete () const override
 Check if the set of states is discrete. More...
 
bool isHybrid () const override
 Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
 
bool isMetricSpace () const override
 Return true if the distance function associated with the space is a metric.
 
bool hasSymmetricDistance () const override
 Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
 
bool hasSymmetricInterpolate () const override
 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
 
void setName (const std::string &name)
 
int getType () const
 
bool includes (const StateSpacePtr &other) const
 
bool includes (const StateSpace *other) const
 
bool covers (const StateSpacePtr &other) const
 
bool covers (const StateSpace *other) const
 
ParamSetparams ()
 
const ParamSetparams () const
 Get the parameters for this space.
 
double getLongestValidSegmentFraction () const 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 returns this length, for this state space, as a fraction of the space's maximum extent.
 
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. More...
 
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 setValidSegmentCountFactor (unsigned int factor) override
 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).
 
unsigned int getValidSegmentCountFactor () const override
 Get the value used to multiply the return value of validSegmentCount().
 
double getLongestValidSegmentLength () const override
 Get the longest valid segment at the time setup() was called.
 
void computeSignature (std::vector< int > &signature) const override
 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.
 
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.
 
void copyState (State *destination, const State *source) const override
 Copy a state to another. The memory of source and destination should NOT overlap. More...
 
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()
 
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.
 
bool equalStates (const State *state1, const State *state2) const override
 Checks whether two states are equal.
 
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. The memory location of state is not required to be different from the memory of either from or to.
 
StateSamplerPtr allocDefaultStateSampler () const override
 Allocate an instance of the default uniform state sampler for this space.
 
StateallocState () const override
 Allocate a state that can store a point in the described space.
 
void freeState (State *state) const override
 Free the memory of the allocated state.
 
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...
 
const double * getValueAddressAtIndex (const State *state, unsigned int index) const
 
const std::vector< ValueLocation > & getValueLocations () const
 
const std::map< std::string, ValueLocation > & getValueLocationsByName () const
 
double * getValueAddressAtLocation (State *state, const ValueLocation &loc) const
 
const double * getValueAddressAtLocation (const State *state, const ValueLocation &loc) const
 
double * getValueAddressAtName (State *state, const std::string &name) const
 
const double * getValueAddressAtName (const State *state, const std::string &name) const
 
void copyToReals (std::vector< double > &reals, const State *source) const override
 Copy all the real values from a state source to the array reals using getValueAddressAtLocation()
 
void copyFromReals (State *destination, const std::vector< double > &reals) const override
 Copy the values from reals to the state destination using getValueAddressAtLocation()
 
void registerProjections () override
 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().
 
void printState (const State *state, std::ostream &out=std::cout) 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 printProjections (std::ostream &out) const override
 Print the list of registered projections. This function is also called by printSettings()
 
void sanityChecks (double zero, double eps, unsigned int flags) const override
 Perform sanity checks for this state space. Throws an exception if failures are found. More...
 
void sanityChecks () const override
 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.
 
StateSamplerPtr allocSubspaceStateSampler (const StateSpace *subspace) const override
 Allocate a sampler that actually samples only components that are part of subspace.
 
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.
 
const StateSpacePtrgetSpace () const
 
- 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...
 
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.
 
StatecloneState (const State *source) const
 Clone a state.
 
virtual StateSamplerPtr allocStateSampler () const
 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.
 
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;.
 
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.
 
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.
 
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

SpaceInformationsi_ {nullptr}
 SpaceInformation associated with this space. Required for early collision checking in manifold traversal.
 
const ConstraintPtr constraint_
 Constraint function that defines the manifold.
 
const unsigned int n_
 Ambient space dimension.
 
const unsigned int k_
 Manifold dimension.
 
double delta_
 Step size when traversing the manifold and collision checking.
 
double lambda_ {ompl::magic::CONSTRAINED_STATE_SPACE_LAMBDA}
 Manifold traversal from x to y is stopped if accumulated distance is greater than d(x,y) times this. Additionally, if d(x, y) is greater than lambda * delta between two points, search is discontinued.
 
bool setup_ {false}
 Whether setup() has been called.
 
- Protected Attributes inherited from ompl::base::WrapperStateSpace
const StateSpacePtr space_
 
- 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.
 
- Static Protected Attributes inherited from ompl::base::StateSpace
static const std::string DEFAULT_PROJECTION_NAME = ""
 The name used for the default projection.
 

Detailed Description

A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambient space, and the constraint is used to inform any manifold related operations. setSpaceInformation() must be called in order for collision checking to be done in tandem with manifold traversal.

Short Description
ConstrainedStateSpace encapsulates the idea of decoupled constrained planning, where the planner and constraint satisfaction methodology are separated. Core to this idea is the augmentation of a state space, rather than the augmentation of a planner. In OMPL, this is implemented as the ConstrainedStateSpace and its concrete implementations: ProjectedStateSpace for projection-based constraint satisfaction, AtlasStateSpace for atlas-based manifold approximation, and TangentBundleStateSpace for a lazy atlas-based approach.

The core benefit of this method is that there is no additional work to make a sampling-based planner plan with constraints (in this case, a differentiable function of a state, implemented in Constraint), enabling mix-and-matching of planners with constraint methodologies, e.g., KPIECE1 with TangentBundleStateSpace, or RRT* with ProjectedStateSpace, and so on.

See Constrained Planning for more details.

External Documentation
The following paper describes the idea of decoupled constrained planning, as implemented in OMPL.

Zachary Kingston, Mark Moll, and Lydia E. Kavraki, Exploring Implicit Spaces for Constrained Sampling-Based Planning, International Journal of Robotics Research, 38(10–11):1151–1178, September 2019. DOI: 10.1177/0278364919868530 PrePrint: [PDF]

For more information on constrained sampling-based planning in general, see the following review paper. The sections on projection- and atlas-based planning describe the methods used in the ProjectedStateSpace, AtlasStateSpace, and TangentBundleStateSpace.

Z. Kingston, M. Moll, and L. E. Kavraki, “Sampling-Based Methods for Motion Planning with Constraints,” Annual Review of Control, Robotics, and Autonomous Systems, 2018. DOI: 10.1146/annurev-control-060117-105226 [PDF].

Definition at line 165 of file ConstrainedStateSpace.h.

Member Enumeration Documentation

◆ SanityChecks

Flags used in a bit mask for constrained state space sanity checks, constraintSanityChecks().

Enumerator
CONSTRAINED_STATESPACE_SAMPLERS 

Check whether state samplers return constraint satisfying samples.

CONSTRAINED_STATESPACE_GEODESIC_SATISFY 

Check whether discrete geodesics satisfy the constraint at all points.

CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY 

Check whether discrete geodesics keep lambda_ * delta_ continuity.

CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE 

Check whether geodesicInterpolate(...) returns constraint satisfying states.

CONSTRAINED_STATESPACE_JACOBIAN 

Check if the constraint's numerical Jacobian approximates its provided Jacobian.

Definition at line 170 of file ConstrainedStateSpace.h.


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