1 #include <ompl/control/SpaceInformation.h>
2 #include <ompl/base/goals/GoalState.h>
3 #include <ompl/base/spaces/SE2StateSpace.h>
4 #include <ompl/control/spaces/RealVectorControlSpace.h>
5 #include <ompl/control/planners/kpiece/KPIECE1.h>
6 #include <ompl/control/planners/rrt/RRT.h>
7 #include <ompl/control/planners/est/EST.h>
8 #include <ompl/control/planners/syclop/SyclopRRT.h>
9 #include <ompl/control/planners/syclop/SyclopEST.h>
10 #include <ompl/control/SimpleSetup.h>
11 #include <ompl/config.h>
12 #include <ompl/extensions/triangle/TriangularDecomposition.h>
15 namespace ob = ompl::base;
16 namespace oc = ompl::control;
35 sampler->sampleUniform(s);
39 std::vector<Polygon> createObstacles()
41 std::vector<Polygon> obst;
66 bool triContains(
double x,
double y,
double ax,
double ay,
double bx,
double by,
double cx,
double cy)
68 if ((x-ax)*(by-ay) - (bx-ax)*(y-ay) > 0.)
70 if ((x-bx)*(cy-by) - (cx-bx)*(y-by) > 0.)
72 if ((x-cx)*(ay-cy) - (ax-cx)*(y-cy) > 0.)
85 double x = se2state->getX();
86 double y = se2state->getY();
87 return si->
satisfiesBounds(state) && !triContains(x,y, -0.5,0.75,-0.75,0.68,-0.5,0.5)
88 && !triContains(x,y, 0,0.5,-0.3,0,0,-0.5)
89 && !triContains(x,y,0,-0.5,0.6,0.6,0,0.5);
97 const oc::RealVectorControlSpace::ControlType *rctrl = control->
as<oc::RealVectorControlSpace::ControlType>();
100 (*pos)[0] + (*rctrl)[0] * duration * cos(rot->value);
103 (*pos)[1] + (*rctrl)[0] * duration * sin(rot->value);
106 rot->value + (*rctrl)[1];
127 cbounds.setLow(-0.3);
128 cbounds.setHigh(0.3);
155 pdef->setStartAndGoalStates(start, goal, 0.1);
164 planner->setProblemDefinition(pdef);
174 pdef->print(std::cout);
184 std::cout <<
"Found solution:" << std::endl;
187 path->print(std::cout);
190 std::cout <<
"No solution found" << std::endl;
194 void planWithSimpleSetup(
void)
211 cbounds.setLow(-0.3);
212 cbounds.setHigh(0.3);
220 ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
223 ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
239 ss.setStartAndGoalStates(start, goal, 0.05);
246 std::cout <<
"Found solution:" << std::endl;
249 ss.getSolutionPath().asGeometric().print(std::cout);
252 std::cout <<
"No solution found" << std::endl;
255 int main(
int,
char **)
257 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
261 std::cout << std::endl << std::endl;
A TriangularDecomposition is a triangulation that ignores obstacles.
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.
TriangularDecomposition(unsigned int dim, const base::RealVectorBounds &b, const std::vector< Polygon > &holes=std::vector< Polygon >())
Constructor. Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional...
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.
A boost shared pointer wrapper for ompl::base::Path.