TriangulationDemo.cpp
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>
13 #include <iostream>
14 
15 namespace ob = ompl::base;
16 namespace oc = ompl::control;
17 
18 // a decomposition is only needed for SyclopRRT and SyclopEST
19 class MyTriangularDecomposition : public oc::TriangularDecomposition
20 {
21 public:
22  MyTriangularDecomposition(const ob::RealVectorBounds& bounds)
23  : oc::TriangularDecomposition(bounds, createObstacles())
24  {
25  setup();
26  }
27  virtual void project(const ob::State* s, std::vector<double>& coord) const
28  {
29  coord.resize(2);
30  coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
31  coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
32  }
33 
34  virtual void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const
35  {
36  sampler->sampleUniform(s);
37  s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
38  }
39 
40  std::vector<Polygon> createObstacles()
41  {
42  std::vector<Polygon> obst;
43  Triangle tri;
44  tri.pts[0].x = -0.5;
45  tri.pts[0].y = 0.75;
46  tri.pts[1].x = -0.75;
47  tri.pts[1].y = 0.68;
48  tri.pts[2].x = -0.5;
49  tri.pts[2].y = 0.5;
50  obst.push_back(tri);
51 
52  Polygon rect(4);
53  rect.pts[0].x = 0.;
54  rect.pts[0].y = 0.5;
55  rect.pts[1].x = -0.3;
56  rect.pts[1].y = 0.;
57  rect.pts[2].x = 0.;
58  rect.pts[2].y = -0.5;
59  rect.pts[3].x = 0.6;
60  rect.pts[3].y = 0.6;
61  obst.push_back(rect);
62 
63  return obst;
64  }
65 };
66 
67 bool triContains(double x, double y, double ax, double ay, double bx, double by, double cx, double cy)
68 {
69  if ((x-ax)*(by-ay) - (bx-ax)*(y-ay) > 0.)
70  return false;
71  if ((x-bx)*(cy-by) - (cx-bx)*(y-by) > 0.)
72  return false;
73  if ((x-cx)*(ay-cy) - (ax-cx)*(y-cy) > 0.)
74  return false;
75  return true;
76 }
77 
78 
79 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
80 {
81  // ob::ScopedState<ob::SE2StateSpace>
82  // cast the abstract state type to the type we expect
84 
85  // check validity of state defined by pos & rot
86  double x = se2state->getX();
87  double y = se2state->getY();
88  return si->satisfiesBounds(state) && !triContains(x,y, -0.5,0.75,-0.75,0.68,-0.5,0.5)
89  && !triContains(x,y, 0,0.5,-0.3,0,0,-0.5)
90  && !triContains(x,y,0,-0.5,0.6,0.6,0,0.5);
91 }
92 
93 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
94 {
98  const oc::RealVectorControlSpace::ControlType *rctrl = control->as<oc::RealVectorControlSpace::ControlType>();
99 
100  result->as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[0] =
101  (*pos)[0] + (*rctrl)[0] * duration * cos(rot->value);
102 
103  result->as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[1] =
104  (*pos)[1] + (*rctrl)[0] * duration * sin(rot->value);
105 
106  result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1)->value =
107  rot->value + (*rctrl)[1];
108 }
109 
110 
111 void planWithSimpleSetup(void)
112 {
113  // construct the state space we are planning in
115 
116  // set the bounds for the R^2 part of SE(2)
117  ob::RealVectorBounds bounds(2);
118  bounds.setLow(-1);
119  bounds.setHigh(1);
120 
121  space->as<ob::SE2StateSpace>()->setBounds(bounds);
122 
123  // create a control space
124  oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
125 
126  // set the bounds for the control space
127  ob::RealVectorBounds cbounds(2);
128  cbounds.setLow(-0.3);
129  cbounds.setHigh(0.3);
130 
131  cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
132 
133  // define a simple setup class
134  oc::SimpleSetup ss(cspace);
135 
136  // set the state propagation routine
137  ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
138 
139  // set state validity checking for this space
140  ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
141 
142  // create a start state
144  start->setX(-0.5);
145  start->setY(0.0);
146  start->setYaw(0.0);
147 
149  goal->setX(0.5);
150 
151  // set the start and goal states
152  ss.setStartAndGoalStates(start, goal, 0.05);
153 
154  oc::TriangularDecomposition* td = new MyTriangularDecomposition(bounds);
155  // print the triangulation to stdout
156  td->print(std::cout);
157 
158  // hand the triangulation to SyclopEST
159  ob::PlannerPtr planner(new oc::SyclopEST(ss.getSpaceInformation(), oc::DecompositionPtr(td)));
160  // hand the SyclopEST planner to SimpleSetup
161  ss.setPlanner(planner);
162 
163  // attempt to solve the problem within ten seconds of planning time
164  ob::PlannerStatus solved = ss.solve(10.0);
165 
166  if (solved)
167  {
168  std::cout << "Found solution:" << std::endl;
169  // print the path to screen
170 
171  ss.getSolutionPath().asGeometric().print(std::cout);
172  }
173  else
174  std::cout << "No solution found" << std::endl;
175 }
176 
177 int main(int, char **)
178 {
179  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
180  planWithSimpleSetup();
181  std::cout << std::endl;
182  return 0;
183 }
A TriangularDecomposition is a triangulation that ignores obstacles.
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:72
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:48
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
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.
Definition: SimpleSetup.h:64
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
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.
State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
A boost shared pointer wrapper for ompl::control::ControlSpace.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
A boost shared pointer wrapper for ompl::base::Planner.
A control space representing Rn.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
SyclopEST is Syclop with EST as its low-level tree planner.
Definition: SyclopEST.h:51
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:50
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
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...
The lower and upper bounds for an Rn space.
A boost shared pointer wrapper for ompl::control::Decomposition.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
Space information containing necessary information for planning with controls. setup() needs to be ca...
TriangularDecomposition(const base::RealVectorBounds &bounds, const std::vector< Polygon > &holes=std::vector< Polygon >(), const std::vector< Polygon > &intRegs=std::vector< Polygon >())
Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional. The underlying mesh will be a conforming Delaunay triangulation. The triangulation will ignore any obstacles, given as a list of polygons. The triangulation will respect the boundaries of any regions of interest, given as a list of polygons. No two obstacles may overlap, and no two regions of interest may overlap.