ConstrainedStateSpace encapsulating a projectionbased methodology for planning with constraints. More...
#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
Public Member Functions  
ProjectedStateSpace (const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)  
Construct an atlas with the specified dimensions.  
~ProjectedStateSpace () override=default  
Destructor.  
StateSamplerPtr  allocDefaultStateSampler () const override 
Allocate the default state sampler for this space.  
StateSamplerPtr  allocStateSampler () const override 
Allocate the previously set state sampler for this space.  
bool  discreteGeodesic (const State *from, const State *to, bool interpolate=false, std::vector< State *> *geodesic=nullptr) const override 
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. More...  
Public Member Functions inherited from ompl::base::ConstrainedStateSpace  
ConstrainedStateSpace (const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)  
Construct a constrained space from an ambientSpace and a constraint.  
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.  
State *  allocState () 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.  
virtual 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_.  
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 State *  geodesicInterpolate (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.  
void  setDelta (const 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, 1t, 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 
ParamSet &  params () 
const ParamSet &  params () 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 noop.  
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.  
State *  allocState () 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 reinstantiated, but their setup() method is still called.  
const StateSpacePtr &  getSpace () const 
Public Member Functions inherited from ompl::base::StateSpace  
StateSpace (const StateSpace &)=delete  
StateSpace &  operator= (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.  
ParamSet &  params () 
Get the parameters for this space.  
const ParamSet &  params () const 
Get the parameters for this space.  
State *  cloneState (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;.  
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.  
State *  getSubstateAtLocation (State *state, const SubstateLocation &loc) const 
Get the substate of state that is pointed to by loc.  
const State *  getSubstateAtLocation (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.  
Additional Inherited Members  
Public Types inherited from ompl::base::ConstrainedStateSpace  
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...  
typedef ompl::base::State  StateType 
Define the type of state allocated by this space.  
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 Attributes inherited from ompl::base::ConstrainedStateSpace  
SpaceInformation *  si_ {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, ProjectionEvaluatorPtr >  projections_ 
List of available projections.  
ParamSet  params_ 
The set of parameters for this space.  
std::vector< ValueLocation >  valueLocationsInOrder_ 
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, ValueLocation >  valueLocationsByName_ 
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, SubstateLocation >  substateLocationsByName_ 
All the known substat locations, by name.  
Static Protected Attributes inherited from ompl::base::StateSpace  
static const std::string  DEFAULT_PROJECTION_NAME = "" 
The name used for the default projection.  
Detailed Description
ConstrainedStateSpace encapsulating a projectionbased methodology for planning with constraints.
 Short Description
 ProjectedStateSpace implements a projectionbased methodology for constrained samplingbased planning, where points in ambient space are projected onto the constraint manifold via a projection operator, which in this case is implemented as a Newton's method within the Constraint.
 External Documentation
 For more information on constrained samplingbased planning using projectionbased methods, see the following review paper. The section on projectionbased methods cites most of the relevant literature.
Z. Kingston, M. Moll, and L. E. Kavraki, “SamplingBased Methods for Motion Planning with Constraints,” Annual Review of Control, Robotics, and Autonomous Systems, 2018. DOI: 10.1146/annurevcontrol060117105226 [PDF].
Definition at line 104 of file ProjectedStateSpace.h.
Member Function Documentation
◆ discreteGeodesic()

overridevirtual 
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.
Public
Implements ompl::base::ConstrainedStateSpace.
Definition at line 76 of file ProjectedStateSpace.cpp.
The documentation for this class was generated from the following files:
 ompl/base/spaces/constraint/ProjectedStateSpace.h
 ompl/base/spaces/constraint/src/ProjectedStateSpace.cpp