Constrained Planning Tutorial {#constrainedPlanningTutorial}

[TOC]

Defining a constrained motion planning problem is easy and very similar to defining an unconstrained planning problem. The primary difference is the need to define a constraint, and the use of a constrained state space, which wraps around an ambient state space. In this example, we will walk through defining a simple constrained planning problem: a point in \f$\mathbb{R}^3\f$ that is constrained to be on the surface of a sphere, giving a constraint function \f$f(q) = \lVert q \rVert - 1\f$. This is very similar to the problem defined by the demo ConstrainedPlanningSphere.

Defining the Constraint

First, let’s define our constraint class. Constraints must inherit from the base class ompl::base::Constraint. Primarily, the function ompl::base::Constraint::function() must be implemented by any concrete implementation of a constraint. A bare-bones version of the sphere constraint we gave above might look like this:

We now have a constraint function that defines a sphere in \f$\mathbb{R}^3\f$! We can visualize the constraint simply as a sphere in \f$\mathbb{R}^3\f$, shown below.

Now we can use this constraint to define a constrained state space.

Constraint Jacobian

However, we can do better. We would like to include an analytical Jacobian for our constraint function, so that planning more efficient. Either we can compute this by hand, or we can use some symbolic solver (e.g., ConstraintGeneration.py shows how to do this with SymPy). Either way, we add to our class the function to compute the Jacobian:

In general, it is highly recommended that you provide an analytic Jacobian for a constrained planning problem, especially for high-dimensional problems.

Projection

One of the primary features of ompl::base::Constraint is the projection function, ompl::base::Constraint::project(). This function takes as input a potential constraint unsatisfying state and maps it onto the constraint manifold, generating a constraint satisfying state. By default, ompl::base::Constraint::project() implements a Newton’s method which performs well in most circumstances.

However, it is possible to override this method with your own projection routine. For example, in the case of our sphere, it could simply normalize the state, placing it on the sphere. Another example would be to use inverse kinematics for a complex robot manipulator.

Constraint Parameters

ompl::base::Constraint affords a two parameters that affect performance. The first is tolerance, which can be set with ompl::base::Constraint::setTolerance(), or via the constructor. Using the constructor, our Sphere class could like like this:

tolerance is used in ompl::base::Constraint::project() to determine when a state satisfies the constraints, and in ompl::base::Constraint::isSatisfied() for the same purpose. For problems that afford lower tolerances (e.g., highly compliant robots), tolerance can be lowered. Lower tolerance values generally simplifies the planning problem, as it is easier to satisfy constraints.

The second is maxIterations, which can be set with ompl::base::Constraint::setMaxIterations(). maxIterations is used in ompl::base::Constraint::project() to limit the number of iterations the projection routine uses, in the case that a satisfying configuration cannot be found. It might be necessary to adjust this value for particularly easy or hard constraint functions to satisfy (decreasing and increasing iterations respectively).

Defining the Constrained State Space

Ambient State Space

Before we can define a constrained state space, we need to define the ambient state space \f$\mathbb{R}^3\f$.

The ambient space is the space over which the constraint is defined, and is used by our constrained state space.

Constraint Instance

We then need to create an instance of our constraint:

The constraint instance is used by our constrained state space.

Constrained State Space

Now that we have both the ambient state space and the constraint, we can define the constrained state space. For this example, we will be using ompl::base::ProjectedStateSpace, which implements a projection operator-based methodology for satisfying constraints. However, we could also just as easily use the other constrained state spaces, ompl::base::AtlasStateSpace or ompl::base::TangentBundleStateSpace, for this problem. We will also be creating a ompl::base::ConstrainedSpaceInformation, which is an augmentation of ompl::base::SpaceInformation with some small modification to take into account constraints.

One of the most important things that ompl::base::ConstrainedSpaceInformation does is call ompl::base::ConstrainedStateSpace::setSpaceInformation, which allows for the manifold traversal to do collision checking in tandem with discrete geodesic computation. Now, we have a constrained state space and space information which we can use for planning.

Defining a Problem

Let’s define a simple problem to solve for this constrained space: traverse the sphere from the south pole to the north pole, avoiding a simple obstacle near the equator. Defining a problem using the constraint framework is as simple as defining an unconstrained problem, and uses the same set of tools. For example, we will be creating a ompl::geometric::SimpleSetup to help with problem definition.

State Validity Checker

Let’s define our state validity checker, which is a simple narrow band around the equator with a hole on one side:

Below, you can see a representation of this obstacle on our sphere.

Start and Goal

Now, let’s also set the start and goal states, the south and north poles of the sphere. Note that for constrained problems, the start and goal states (if using exact states) must satisfy the constraint function. If they do not, then problems will occur.

Planner

Finally, we can add a planner like normal. Let’s use ompl::geometric::PRM, but any other planner in ompl::geometric would do.

Solving a Problem

With everything now in place, we can set everything up to get ready for planning:

Same as how defining a problem is similar for constrained and unconstrained problems, solving a problem is also very similar. Let’s give our planner 5 seconds of time and see what happens:

Interpolation

Note that in general the initial path that you find is not “continuous”. That is, the distance between states in the path (especially after simplification) can be very far apart! If you want a path that has close, intermediate constraint satisfying states, you need to interpolate the path. In the code above, this is achieved with ompl::geometric::PathGeometric::interpolate().

In Summary

With all that, we’ve now solved a constrained motion planning problem on a sphere. A resulting motion graph for PRM could look something like this, with the simplified solution path highlighted in yellow:

Overall, planning with constraints is simple to setup and use. Beyond requiring you to define a constraint function and wrap your ambient space in a constrained state space, OMPL works and feels like normal. You can read more in general about constrained planning on the main page.