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> 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);
122 std::cout <<
"Found solution:" << std::endl;
125 path->print(std::cout);
128 std::cout <<
"No solution found" << std::endl;
131 void 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;
178 int main(
int ,
char ** )
180 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
184 std::cout << std::endl << std::endl;
186 planWithSimpleSetup();
Definition of a scoped state.
Create the set of classes typically needed to solve a geometric problem.
const T * as() const
Cast this instance to a desired type.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
ompl::base::State StateType
Define the type of state allocated by this space.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The lower and upper bounds for an Rn space.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
This namespace contains code that is specific to planning under geometric constraints.
A shared pointer wrapper for ompl::base::Path.