Geometric Planning for a Rigid Body in 3D

This tutorial shows how to plan for a rigid body in 3D. We show how to do this in two ways: with and without the ompl::geometric::SimpleSetup class. The main difference is that in the latter case ompl::base::SpaceInformation and ompl::base::ProblemDefinition need to be explicitly instantiated. Furthermore, the planner to be used must be explicitly instantiated as well. The recommended approach is using ompl::geometric::SimpleSetup as this is less bug prone and does not limit the functionality of the code in any way.

Setting up geometric planning for a rigid body in 3D requires the following steps:

• identify the space we are planning in: SE(3)
• select a corresponding state space from the available ones, or implement one. For SE(3), the ompl::base::SE3StateSpace is appropriate.
• since SE(3) contains an R3 component, we need to define bounds.
• define the notion of state validity.
• define start states and a goal representation.

Once these steps are complete, the specification of the problem is conceptually done. The set of classes that allow the instantiation of this specification is shown below.

# Using the ompl::geometric::SimpleSetup Class

Assuming the following namespace definitions:

namespace ob = ompl::base;
namespace og = ompl::geometric;

And a state validity checking function defined like this:

bool isStateValid(const ob::State *state)

We first create an instance of the state space we are planning in.

void planWithSimpleSetup()
{
// construct the state space we are planning in
auto space(std::make_shared<ob::SE3StateSpace>());

We then set the bounds for the R3 component of this state space:

bounds.setLow(-1);
bounds.setHigh(1);
space->setBounds(bounds);

Create an instance of ompl::geometric::SimpleSetup. Instances of ompl::base::SpaceInformation, and ompl::base::ProblemDefinition are created internally.

og::SimpleSetup ss(space);

Set the state validity checker

ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });

Create a random start state:

ob::ScopedState<> start(space);
start.random();

And a random goal state:

ob::ScopedState<> goal(space);
goal.random();

Set these states as start and goal for SimpleSetup.

ss.setStartAndGoalStates(start, goal);

We can now try to solve the problem. This will also trigger a call to ompl::geometric::SimpleSetup::setup() and create a default instance of a planner, since we have not specified one. Furthermore, ompl::base::Planner::setup() is called, which in turn calls ompl::base::SpaceInformation::setup(). This chain of calls will lead to computation of runtime parameters such as the state validity checking resolution. This call returns a value from ompl::base::PlannerStatus which describes whether a solution has been found within the specified amount of time (in seconds). If this value can be cast to true, a solution was found.

ob::PlannerStatus solved = ss.solve(1.0);

If a solution has been found, we can optionally simplify it and the display it

if (solved)
{
std::cout << "Found solution:" << std::endl;
// print the path to screen
ss.simplifySolution();
ss.getSolutionPath().print(std::cout);
}

# Without ompl::geometric::SimpleSetup

Assuming the following namespace definitions:

namespace ob = ompl::base;
namespace og = ompl::geometric;

And a state validity checking function defined like this:

bool isStateValid(const ob::State *state)

We first create an instance of the state space we are planning in.

void plan()
{
// construct the state space we are planning in
auto space(std::make_shared<ob::SE3StateSpace>());

We then set the bounds for the R3 component of this state space:

bounds.setLow(-1);
bounds.setHigh(1);
space->setBounds(bounds);

Create an instance of ompl::base::SpaceInformation for the state space

auto si(std::make_shared<ob::SpaceInformation>(space));

Set the state validity checker

si->setStateValidityChecker(isStateValid);

Create a random start state:

ob::ScopedState<> start(space);
start.random();

And a random goal state:

ob::ScopedState<> goal(space);
goal.random();

Create an instance of ompl::base::ProblemDefinition

auto pdef(std::make_shared<ob::ProblemDefinition>(si));

Set the start and goal states for the problem definition.

pdef->setStartAndGoalStates(start, goal);

Create an instance of a planner

auto planner(std::make_shared<og::RRTConnect>(si));

Tell the planner which problem we are interested in solving

planner->setProblemDefinition(pdef);

Make sure all the settings for the space and planner are in order. This will also lead to the runtime computation of the state validity checking resolution.

planner->setup();

We can now try to solve the problem. This call returns a value from ompl::base::PlannerStatus which describes whether a solution has been found within the specified amount of time (in seconds). If this value can be cast to true, a solution was found.

ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);

If a solution has been found, we display it. Simplification could be done, but we would need to create an instance of ompl::geometric::PathSimplifier.

if (solved)
{
// get the goal representation from the problem definition (not the same as the goal state)
// and inquire about the found path
ob::PathPtr path = pdef->getSolutionPath();
std::cout << "Found solution:" << std::endl;
// print the path to screen
path->print(std::cout);
}
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:77
Definition of an abstract state.
Definition: State.h:110
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:124
A class to store the exit status of Planner::solve()
Definition of a scoped state.
Definition: ScopedState.h:117
The lower and upper bounds for an Rn space.