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>
45 namespace ob = ompl::base;
46 namespace og = ompl::geometric;
63 return (
const void*)rot != (
const void*)pos;
82 si->setStateValidityChecker(boost::bind(&isStateValid, _1));
96 pdef->setStartAndGoalStates(start, goal);
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(
void)
147 ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
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();
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
CompoundState StateType
Define the type of state allocated by this state space.
State StateType
Define the type of state allocated by this space.
const T * as(void) const
Cast this instance to a desired type.
Create the set of classes typically needed to solve a geometric problem.
A boost shared pointer wrapper for ompl::base::Planner.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
The lower and upper bounds for an Rn space.
A state space representing SE(3)
A boost shared pointer wrapper for ompl::base::Path.