37 #include <ompl/control/SpaceInformation.h>
38 #include <ompl/base/goals/GoalState.h>
39 #include <ompl/base/spaces/SE2StateSpace.h>
40 #include <ompl/control/spaces/RealVectorControlSpace.h>
41 #include <ompl/control/planners/kpiece/KPIECE1.h>
42 #include <ompl/control/planners/rrt/RRT.h>
43 #include <ompl/control/planners/est/EST.h>
44 #include <ompl/control/planners/syclop/SyclopRRT.h>
45 #include <ompl/control/planners/syclop/SyclopEST.h>
46 #include <ompl/control/planners/pdst/PDST.h>
47 #include <ompl/control/planners/syclop/GridDecomposition.h>
48 #include <ompl/control/SimpleSetup.h>
49 #include <ompl/config.h>
52 namespace ob = ompl::base;
53 namespace oc = ompl::control;
72 sampler->sampleUniform(s);
93 return si->
satisfiesBounds(state) && (
const void*)rot != (
const void*)pos;
101 const double* ctrl = control->
as<oc::RealVectorControlSpace::ControlType>()->values;
104 pos[0] + ctrl[0] * duration * cos(rot),
105 pos[1] + ctrl[0] * duration * sin(rot));
107 rot + ctrl[1] * duration);
128 cbounds.setLow(-0.3);
129 cbounds.setHigh(0.3);
156 pdef->setStartAndGoalStates(start, goal, 0.1);
167 planner->setProblemDefinition(pdef);
177 pdef->print(std::cout);
187 std::cout <<
"Found solution:" << std::endl;
190 path->print(std::cout);
193 std::cout <<
"No solution found" << std::endl;
197 void planWithSimpleSetup(
void)
214 cbounds.setLow(-0.3);
215 cbounds.setHigh(0.3);
223 ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
226 ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
242 ss.setStartAndGoalStates(start, goal, 0.05);
251 std::cout <<
"Found solution:" << std::endl;
254 ss.getSolutionPath().printAsMatrix(std::cout);
257 std::cout <<
"No solution found" << std::endl;
260 int main(
int,
char **)
262 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
268 planWithSimpleSetup();
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
Definition of an abstract control.
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
Create the set of classes typically needed to solve a control problem.
CompoundState StateType
Define the type of state allocated by this state space.
A GridDecomposition is a Decomposition implemented using a grid.
State StateType
Define the type of state allocated by this space.
const T * as(void) const
Cast this instance to a desired type.
A boost shared pointer wrapper for ompl::control::ControlSpace.
virtual void sampleFullState(const base::StateSamplerPtr &sampler, const std::vector< double > &coord, base::State *s) const =0
Samples a State using a projected coordinate and a StateSampler.
A boost shared pointer wrapper for ompl::base::Planner.
A control space representing Rn.
A state space representing SE(2)
SyclopEST is Syclop with EST as its low-level tree planner.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition...
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.
const T * as(void) const
Cast this instance to a desired type.
A boost shared pointer wrapper for ompl::control::Decomposition.
GridDecomposition(unsigned int len, unsigned int dim, const base::RealVectorBounds &b)
Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length...
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
A boost shared pointer wrapper for ompl::base::Path.