Class ConstrainedStateSpace

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Derived Types

Class Documentation

class ConstrainedStateSpace : public ompl::base::WrapperStateSpace

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.

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.

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.

See constrainedPlanning for more details.

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]

External Documentation

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

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].

Subclassed by ompl::base::AtlasStateSpace, ompl::base::ProjectedStateSpace

inline virtual 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.

virtual void setup() override

Final setup for the space.

virtual void clear()

Clear any allocated memory from the state space.

virtual 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.

virtual void sanityChecks() const override

Perform both constrained and regular sanity checks.

inline 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_.

Constrained Planning

virtual 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 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.

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.

inline 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.

inline double getDelta() const

Get delta, the step size across the manifold.

inline double getLambda() const

Get lambda (see setLambda()).

inline unsigned int getAmbientDimension() const

Returns the dimension of the ambient space.

inline unsigned int getManifoldDimension() const

Returns the dimension of the manifold.

inline const ConstraintPtr getConstraint() const

Returns the constraint that defines the underlying manifold.

Public Types

enum SanityChecks

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

Values:

enumerator CONSTRAINED_STATESPACE_SAMPLERS

Check whether state samplers return constraint satisfying samples.

enumerator CONSTRAINED_STATESPACE_GEODESIC_SATISFY

Check whether discrete geodesics satisfy the constraint at all points.

enumerator CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY

Check whether discrete geodesics keep lambda_ * delta_ continuity.

enumerator CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE

Check whether geodesicInterpolate(…) returns constraint satisfying states.

enumerator CONSTRAINED_STATESPACE_JACOBIAN

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

Public Functions

ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)

Construct a constrained space from an ambientSpace and a constraint.

Protected Attributes

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.

class StateType : public ompl::base::WrapperStateSpace::StateType, public Eigen::Map<Eigen::VectorXd>

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().

Subclassed by ompl::base::AtlasStateSpace::StateType

Public Functions

inline StateType(const ConstrainedStateSpace *space)

Constructor. Requires space to setup information about underlying state.

inline void copy(const Eigen::Ref<const Eigen::VectorXd> &other)

Copy the contents from a vector into this state. Uses the underlying copy operator used by Eigen for dense vectors.