37#include <ompl/base/SpaceInformation.h>
38#include <ompl/base/spaces/SE3StateSpace.h>
39#include <ompl/geometric/planners/rrt/RRTConnect.h>
40#include <ompl/geometric/SimpleSetup.h>
42#include <ompl/config.h>
51 const auto *se3state = state->
as<ob::SE3StateSpace::StateType>();
54 const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
57 const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
63 return (
const void*)rot != (
const void*)pos;
69 auto space(std::make_shared<ob::SE3StateSpace>());
76 space->setBounds(bounds);
79 auto si(std::make_shared<ob::SpaceInformation>(space));
82 si->setStateValidityChecker(isStateValid);
93 auto pdef(std::make_shared<ob::ProblemDefinition>(si));
96 pdef->setStartAndGoalStates(start, goal);
99 auto planner(std::make_shared<og::RRTConnect>(si));
102 planner->setProblemDefinition(pdef);
109 si->printSettings(std::cout);
112 pdef->print(std::cout);
121 ob::PathPtr path = pdef->getSolutionPath();
122 std::cout <<
"Found solution:" << std::endl;
125 path->print(std::cout);
128 std::cout <<
"No solution found" << std::endl;
131void planWithSimpleSetup()
134 auto space(std::make_shared<ob::SE3StateSpace>());
141 space->setBounds(bounds);
147 ss.setStateValidityChecker([](
const ob::State *state) {
return isStateValid(state); });
158 ss.setStartAndGoalStates(start, goal);
169 std::cout <<
"Found solution:" << std::endl;
171 ss.simplifySolution();
172 ss.getSolutionPath().print(std::cout);
175 std::cout <<
"No solution found" << std::endl;
178int main(
int ,
char ** )
180 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
184 std::cout << std::endl << std::endl;
186 planWithSimpleSetup();
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
Create the set of classes typically needed to solve a geometric problem.
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.
A class to store the exit status of Planner::solve()