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;
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48

And a state validity checking function defined like this:

bool isStateValid(const ob::State *state)
Definition of an abstract state.
Definition: State.h:50

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);
The lower and upper bounds for an Rn space.

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

og::SimpleSetup ss(space);
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63

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();
Definition of a scoped state.
Definition: ScopedState.h:57

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);
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49

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);
}