All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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(2, bounds, createObstacles())
24  {
25  }
26  virtual void project(const ob::State* s, std::vector<double>& coord) const
27  {
28  coord.resize(2);
29  coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
30  coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
31  }
32 
33  virtual void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const
34  {
35  sampler->sampleUniform(s);
36  s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
37  }
38 
39  std::vector<Polygon> createObstacles()
40  {
41  std::vector<Polygon> obst;
42  Triangle tri;
43  tri.pts[0].x = -0.5;
44  tri.pts[0].y = 0.75;
45  tri.pts[1].x = -0.75;
46  tri.pts[1].y = 0.68;
47  tri.pts[2].x = -0.5;
48  tri.pts[2].y = 0.5;
49  obst.push_back(tri);
50 
51  Polygon rect(4);
52  rect.pts[0].x = 0.;
53  rect.pts[0].y = 0.5;
54  rect.pts[1].x = -0.3;
55  rect.pts[1].y = 0.;
56  rect.pts[2].x = 0.;
57  rect.pts[2].y = -0.5;
58  rect.pts[3].x = 0.6;
59  rect.pts[3].y = 0.6;
60  obst.push_back(rect);
61 
62  return obst;
63  }
64 };
65 
66 bool triContains(double x, double y, double ax, double ay, double bx, double by, double cx, double cy)
67 {
68  if ((x-ax)*(by-ay) - (bx-ax)*(y-ay) > 0.)
69  return false;
70  if ((x-bx)*(cy-by) - (cx-bx)*(y-by) > 0.)
71  return false;
72  if ((x-cx)*(ay-cy) - (ax-cx)*(y-cy) > 0.)
73  return false;
74  return true;
75 }
76 
77 
78 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
79 {
80  // ob::ScopedState<ob::SE2StateSpace>
81  // cast the abstract state type to the type we expect
83 
84  // check validity of state defined by pos & rot
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);
90 }
91 
92 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
93 {
97  const oc::RealVectorControlSpace::ControlType *rctrl = control->as<oc::RealVectorControlSpace::ControlType>();
98 
99  result->as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[0] =
100  (*pos)[0] + (*rctrl)[0] * duration * cos(rot->value);
101 
102  result->as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[1] =
103  (*pos)[1] + (*rctrl)[0] * duration * sin(rot->value);
104 
105  result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1)->value =
106  rot->value + (*rctrl)[1];
107 }
108 
109 void plan(void)
110 {
111 
112  // construct the state space we are planning in
114 
115  // set the bounds for the R^2 part of SE(2)
116  ob::RealVectorBounds bounds(2);
117  bounds.setLow(-1);
118  bounds.setHigh(1);
119 
120  space->as<ob::SE2StateSpace>()->setBounds(bounds);
121 
122  // create a control space
123  oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
124 
125  // set the bounds for the control space
126  ob::RealVectorBounds cbounds(2);
127  cbounds.setLow(-0.3);
128  cbounds.setHigh(0.3);
129 
130  cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
131 
132  // construct an instance of space information from this control space
133  oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
134 
135  // set state validity checking for this space
136  si->setStateValidityChecker(boost::bind(&isStateValid, si.get(), _1));
137 
138  // set the state propagation routine
139  si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
140 
141  // create a start state
143  start->setX(-0.5);
144  start->setY(0.0);
145  start->setYaw(0.0);
146 
147  // create a goal state
149  goal->setX(0.5);
150 
151  // create a problem instance
153 
154  // set the start and goal states
155  pdef->setStartAndGoalStates(start, goal, 0.1);
156 
157  // create a planner for the defined space
158  oc::TriangularDecomposition* td = new MyTriangularDecomposition(bounds);
159  oc::DecompositionPtr decomp(td);
160  ob::PlannerPtr planner(new oc::SyclopEST(si, decomp));
161  //ob::PlannerPtr planner(new oc::SyclopRRT(si, decomp));
162 
163  // set the problem we are trying to solve for the planner
164  planner->setProblemDefinition(pdef);
165 
166  // perform setup steps for the planner
167  planner->setup();
168 
169 
170  // print the settings for this space
171  si->printSettings(std::cout);
172 
173  // print the problem settings
174  pdef->print(std::cout);
175 
176  // attempt to solve the problem within one second of planning time
177  ob::PlannerStatus solved = planner->solve(10.0);
178 
179  if (solved)
180  {
181  // get the goal representation from the problem definition (not the same as the goal state)
182  // and inquire about the found path
183  ob::PathPtr path = pdef->getSolutionPath();
184  std::cout << "Found solution:" << std::endl;
185 
186  // print the path to screen
187  path->print(std::cout);
188  }
189  else
190  std::cout << "No solution found" << std::endl;
191 }
192 
193 
194 void planWithSimpleSetup(void)
195 {
196  // construct the state space we are planning in
198 
199  // set the bounds for the R^2 part of SE(2)
200  ob::RealVectorBounds bounds(2);
201  bounds.setLow(-1);
202  bounds.setHigh(1);
203 
204  space->as<ob::SE2StateSpace>()->setBounds(bounds);
205 
206  // create a control space
207  oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
208 
209  // set the bounds for the control space
210  ob::RealVectorBounds cbounds(2);
211  cbounds.setLow(-0.3);
212  cbounds.setHigh(0.3);
213 
214  cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
215 
216  // define a simple setup class
217  oc::SimpleSetup ss(cspace);
218 
219  // set the state propagation routine
220  ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
221 
222  // set state validity checking for this space
223  ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
224 
225  // create a start state
227  start->setX(-0.5);
228  start->setY(0.0);
229  start->setYaw(0.0);
230 
231  // create a goal state; use the hard way to set the elements
233  (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
234  (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
235  (*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;
236 
237 
238  // set the start and goal states
239  ss.setStartAndGoalStates(start, goal, 0.05);
240 
241  // attempt to solve the problem within one second of planning time
242  ob::PlannerStatus solved = ss.solve(10.0);
243 
244  if (solved)
245  {
246  std::cout << "Found solution:" << std::endl;
247  // print the path to screen
248 
249  ss.getSolutionPath().asGeometric().print(std::cout);
250  }
251  else
252  std::cout << "No solution found" << std::endl;
253 }
254 
255 int main(int, char **)
256 {
257  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
258 
259  plan();
260 
261  std::cout << std::endl << std::endl;
262 
263  //planWithSimpleSetup();
264 
265  return 0;
266 }
A TriangularDecomposition is a triangulation that ignores obstacles.
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:48
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:63
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:543
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.
Definition: StateSpace.h:78
const T * as(void) const
Cast this instance to a desired type.
Definition: State.h:74
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.
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
A boost shared pointer wrapper for ompl::control::SpaceInformation.
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.
Definition: Control.h:72
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
A boost shared pointer wrapper for ompl::control::Decomposition.
Space information containing necessary information for planning with controls. setup() needs to be ca...
A boost shared pointer wrapper for ompl::base::Path.