Constrained Planning {#constrainedPlanning}

[TOC]

Sometimes, the motion of a robot is constrained by its task or inherent kinematic properties. For example, a robot might want to keep a cup of water level, remain in contact with a surface to write on it, or follow some curve in space that corresponds to a task. In each of these cases, the constraint on the robot’s motion is defined by some function, \f$f(q) : \mathcal{Q} \rightarrow \mathbb{R}^n\f$, which maps the robot’s state space \f$\mathcal{Q}\f$ onto a real vector value \f$\mathbb{R}^n\f$. The constraint is considered satisfied whenever \f$f(q) = \mathbf{0}\f$. For example, to keep the cup level, the angular distance of the cup’s axis to the upright z-axis could be the constraint function. In addition, a constraint satisfying motion satisfies \f$f(q) = \mathbf{0}\f$ at every point along the path. Constrained motion planning addresses the question of how to find a constraint satisfying motion, while still avoiding obstacles or achieving other objectives.

In general, motion planners are not aware of constraints, and will generate paths that do not satisfy the constraint. This is because the submanifold of constraint satisfying configurations \f$X = { q \in \mathcal{Q} \mid f(q) = \mathbf{0} }\f$ is lower-dimensional compared to the state space of the robot, and thus near impossible to sample from. To plan constraint satisfying motion, OMPL provides a means to augment the state space of a robot with knowledge of the constraint, representing \f$X\f$ as a state space. Any motion plan generated using this augmented state space will satisfy the constraint, as the primitive operations used by motions planners (e.g., ompl::base::StateSpace::interpolate) automatically generate constraint satisfying states. The constrained planning framework enables any sampling-based planner (included asymptotically optimal planners) to plan while respecting a constraint function.

You can represent a constraint function using ompl::base::Constraint, where you must implement the function \f$f\f$, and optionally the analytic Jacobian of the constraint function. If no analytic Jacobian is provided, a numerical finite central difference routine is used to approximate the Jacobian. However, this is very computationally intensive, and providing an analytic derivative is preferred. We provide a simple script ConstraintGeneration.py that uses the SymPy Python library for symbolic differentiation of constraints, and can automatically generate constraint code that can be used in your programs. There is also ompl::base::ConstraintIntersection, which allows for composition of multiple constraints together that must all be satisfied.

There are currently three provided representations of constrained state spaces, each of which inherits from ompl::base::ConstrainedStateSpace. Each of these methods implements a different way to sample and traverse the underlying submanifold. The three augmented state spaces are:

ompl::base::ProjectedStateSpace, a constrained state space that uses a projection operator to find constraint satisfying motion.
ompl::base::AtlasStateSpace, a constrained state space that, while planning, builds a piecewise linear approximation of \f$X\f$ using tangent spaces. This approximation is called an atlas, and is used to guide planning.
ompl::base::TangentBundleStateSpace, a constrained state space similar to ompl::base::AtlasStateSpace, but lazily evaluates the atlas.

Each of these state spaces has their own documentation that can be viewed on their page. The augmented state space approach taken by OMPL was presented in a 2019 paper in the International Journal of Robotics Research. More information about constrained motion planning is presented in this review paper.

You can view an example of how to use the constrained planning framework in this tutorial!

Limitations

In order for the constrained planning framework to work, your underlying state space and constraint function must satisfy a few assumptions.

Contiguous Memory

As an implementation detail, the memory allocated in a state by the underlying state space must be a contiguous array of double values. For example, ompl::base::RealVectorStateSpace, or the state space implemented in the kinematic chain benchmark both allocate a contiguous array of double values. This detail is required by the various constrained state spaces and constraint function in order to view them as Eigen::VectorXds, using Eigen::Map<Eigen::VectorXd>. Note that ompl::base::ConstrainedStateSpace::StateType derives from this class, for convenience.

However, this assumption prevents the ompl::base::CompoundStateSpace from being used by the constrained planning framework, as state allocation does not guarantee contiguity.

Constraint Differentiability

In general, your constraint function should be a continuous and differentiable function of the robot’s state. Singularities in the constraint function can cause bad behavior by the underlying constraint satisfaction methods. ompl::base::AtlasStateSpace and ompl::base::TangentBundleStateSpace both will treat singularities as obstacles in the planning process.

Required Interpolation

If you want a path that satisfies constraints and is potentially executable by a real system, you will want to interpolate whatever path (simplified or un-simplified) you find. This can be done by simply calling ompl::geometric::PathGeometric::interpolate(). The interpolated path will be constraint satisfying, as the interpolate routine uses the primitives afforded by the constrained state space. However, there can potentially be issues at this step, as interpolation can fail.

Interpolation Failures

Currently, each of the constrained state spaces implements interpolation on the constraint submanifold via computing a discrete geodesic between the two query points. The discrete geodesic is a sequence of close (to approximate continuity), constraint satisfying states between two query points. The distance between each point in the discrete geodesic is tuned by the “delta” parameter in ompl::base::ConstrainedStateSpace::setDelta(). How this discrete geodesic is computed is key to how constrained state space operates, as it is used ubiquitously throughout the code (e.g., interpolation, collision checking, motion validation, and others).

Due to the nature of how these routines are implemented, it is possible for computation of the discrete geodesic to fail, thus causing potentially unexpected results from whatever overlying routine requested a discrete geodesic. These failures can be the result of singularities in the constraint, high curvature of the submanifold, and various other issues. However, interpolation in “regular” state spaces does not generally fail as they are analytic, such as linear interpolation in ompl::base::RealVectorStateSpace; hence, ompl::base::StateStace::interpolate() is assumed to always be successful. As a result, some unexpected behavior can be seen if interpolation fails during planning with a constrained state space. Increasing or decreasing the “delta” parameter in ompl::base::ConstrainedStateSpace, increasing or decreasing the constraint satisfaction tolerance, and other hyperparameter tuning can fix these problems.

Hyperparameter Sensitivity

The constrained state spaces, in general, are sensitive to the tuning of their various hyperparameters. Some reasonable defaults are set at start, but many constrained planning problems will have different characteristics of the underlying submanifold, and as such different parameters may be needed. Some basic rules-of-thumb are provided below:

For high-dimensional ambient state spaces, many parameters can be increased in magnitude. A brief non-exhaustive list follows.
- Constraint solving tolerance can be increased with ompl::base::Constraint::setTolerance()
- Valid step size for manifold traversal with ompl::base::ConstrainedStateSpace::setDelta()
- Many atlas related parameters for ompl::base::AtlasStateSpace, such as \f$\epsilon, \rho\f$, and exploration.
Generally, the step size for manifold traversal is related to the relative curvature of the underlying constraint submanifold. Less curvy submanifolds can permit larger step sizes (i.e., if the constraint defines a hyperplane, you can use a large step size). Play around with this value if speed is a concern, as the larger the step size is, the less time is spent traversing the manifold.
For the atlas- and tangent bundle-based spaces, planners that rely on uniform sampling of the space may require a high exploration parameter so the constraint submanifold becomes covered quicker (e.g., BIT*). Additionally, planners that rely on ompl::base::StateSampler::samplerNear() may also require a high exploration or \f$\rho\f$ parameter in order to expand effectively.
And many others! In general, the projection-based space is less sensitive to poor parameter tuning than atlas- or tangent bundle-based space, and as such is a good starting point to validate whether a constrained planning problem is feasible.

Additional Notes

Constraint Projection versus Projection Evaluator

Within ompl::base::Constraint, there is a projection function ompl::base::Constraint::project() which maps a potentially constraint unsatisfying configuration to the constraint manifold. By default, ompl::base::Constraint::project() implements a Newton’s method which performs well in most circumstances. Note that it is possible to override this method with your own projection routine, e.g., inverse kinematics.

You might notice that there also exists ompl::base::ProjectionEvaluator::project() This method is used by a few planners to estimate the coverage of the free space (see this tutorial for more information), as it is a “projection” into a lower-dimensional space. Although similar sounding, this concept is orthogonal to the concept of projection as used in ompl::base::Constraint, which projects states into the lower-dimensional constraint manifold. In fact, one can use planners that use a ompl::base::ProjectionEvaluator (e.g., ompl::geometric::KPIECE1, ompl::geometric::ProjEST) in tandem with the constrained planning framework.

By default, the constrained state spaces will use ompl::base::WrapperProjectionEvaluator to access the underlying state space’s default projection evaluator. However, if you know anything about the structure of you problem, you should implement your own projection evaluator for performance. Each of the constrained planning demos has an example of a projection evaluator that can be used with constrained planning (e.g., SphereProjection, in ConstrainedPlanningSphere).

Want to learn more?

Tutorials

Check out the tutorial, which shows how to set up a constrained planning problem in R³.

Demos

The examples (plus more) from the paper that presented the constrained planning framework are available as demo programs. All of the demos are in the ompl/demos/constraint folder. Each of these demos supports planning for an individual planner as well as benchmarking, and complete configurability of the hyperparameters of the constrained space. Each of these demos comes with a way to visualize the results of planning. The sphere, torus, and implicit kinematic chain demos each come with a Blender file (.blend) that visualize the results of planning in 3-D. These are all in ompl/demos/constraint/visualization.

ConstrainedPlanningSphere [Python version]. Plan for a point in R³ constrained to the surface of a sphere, with narrow obstacles it must traverse. A similar scenario is explained in the tutorial. The Blender file ConstrainedPlanningSphere.blend contains a script that visualized the motion graph, original and simplified paths, and the atlas generated while planning if ompl::base::AtlasStateSpace or ompl::base::TangentBundleStateSpace were used. Simply change the path in the script window to the directory where the output of the demo was placed and hit “Run Script”. The Python module NetworkX is required to visualize the graph, make sure your Blender’s Python path is correctly set so it can find the module. Note that you must run the demo with -o to dump output.
ConstrainedPlanningTorus [Python version]. Plan for a point in R³ constrained to the surface of a torus. A “maze” image is loaded (some examples are provided in ompl/demos/constraint/mazes) to use as the set of obstacles to traverse. The start and goal point are red and green pixels in the images respectively. The Blender file ConstrainedPlanningTorus.blend contains a script to visualize the motion graph, simplified path, the atlas generated by planning, and the maze projected upon the torus. Similar to the sphere example, change the path in the script window and “Run Script” to see results.
ConstrainedPlanningImplicitChain [Python version]. Plan for a set of balls, each in R³, constrained to be a unit distance apart. This imposes spherical kinematics on the balls, implicitly defining a kinematic chain. The Blender file ConstrainedPlanningImplicitChain.blend contains a script to show an animation of the simplified path . Similar to the sphere example, change the path in the script window and “Run Script” to see results.
ConstrainedPlanningImplicitParallel. Plan for a parallel robot made of many implicit chains. The Blender file ConstrainedPlanningImplicitParallel.blend contains a script to show an animation of the simplified path. Similar to the sphere example, change the path in the script window and “Run Script” to see results.
ConstrainedPlanningKinematicChain. Similar to the kinematic chain benchmark demo, but with a constraint that only allows the tip of the manipulator to move along a line.