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.
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.
namespace ob = ompl::base; namespace og = ompl::geometric;
bool isStateValid(const ob::State *state)
void planWithSimpleSetup(void) { // construct the state space we are planning in ob::StateSpacePtr space(new ob::SE3StateSpace());
ob::RealVectorBounds bounds(3); bounds.setLow(-1); bounds.setHigh(1); space->as<ob::SE3StateSpace>()->setBounds(bounds);
og::SimpleSetup ss(space);
ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
ob::ScopedState<> start(space); start.random();
ob::ScopedState<> goal(space); goal.random();
ss.setStartAndGoalStates(start, goal);
bool solved = ss.solve(1.0);
if (solved) { std::cout << "Found solution:" << std::endl; // print the path to screen ss.simplifySolution(); ss.getSolutionPath().print(std::cout); }
namespace ob = ompl::base; namespace og = ompl::geometric;
bool isStateValid(const ob::State *state)
void plan(void) { // construct the state space we are planning in ob::StateSpacePtr space(new ob::SE3StateSpace());
ob::RealVectorBounds bounds(3); bounds.setLow(-1); bounds.setHigh(1); space->as<ob::SE3StateSpace>()->setBounds(bounds);
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
si->setStateValidityChecker(boost::bind(&isStateValid, _1));
ob::ScopedState<> start(space); start.random();
ob::ScopedState<> goal(space); goal.random();
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
pdef->setStartAndGoalStates(start, goal);
ob::PlannerPtr planner(new og::RRTConnect(si));
planner->setProblemDefinition(pdef);
planner->setup();
bool solved = planner->solve(1.0);
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->getGoal()->getSolutionPath(); std::cout << "Found solution:" << std::endl; // print the path to screen path->print(std::cout); }