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  void project(const ob::State* s, std::vector<double>& coord) const override
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  void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const override
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
83  const auto *se2state = state->as<ob::SE2StateSpace::StateType>();
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 {
95  const auto *se2state = start->as<ob::SE2StateSpace::StateType>();
96  const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
97  const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
98  const auto *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()
112 {
113  // construct the state space we are planning in
114  auto space(std::make_shared<ob::SE2StateSpace>());
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->setBounds(bounds);
122 
123  // create a control space
124  auto cspace(std::make_shared<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->setBounds(cbounds);
132 
133  // define a simple setup class
134  oc::SimpleSetup ss(cspace);
135 
136  // set the state propagation routine
137  ss.setStatePropagator(propagate);
138 
139  // set state validity checking for this space
140  oc::SpaceInformation *si = ss.getSpaceInformation().get();
141  ss.setStateValidityChecker(
142  [si](const ob::State *state) { return isStateValid(si, state); });
143 
144  // create a start state
146  start->setX(-0.5);
147  start->setY(0.0);
148  start->setYaw(0.0);
149 
151  goal->setX(0.5);
152 
153  // set the start and goal states
154  ss.setStartAndGoalStates(start, goal, 0.05);
155 
156  auto td(std::make_shared<MyTriangularDecomposition>(bounds));
157  // print the triangulation to stdout
158  td->print(std::cout);
159 
160  // hand the triangulation to SyclopEST
161  auto planner(std::make_shared<oc::SyclopEST>(ss.getSpaceInformation(), td));
162  // hand the SyclopEST planner to SimpleSetup
163  ss.setPlanner(planner);
164 
165  // attempt to solve the problem within ten seconds of planning time
166  ob::PlannerStatus solved = ss.solve(10.0);
167 
168  if (solved)
169  {
170  std::cout << "Found solution:" << std::endl;
171  // print the path to screen
172 
173  ss.getSolutionPath().asGeometric().print(std::cout);
174  }
175  else
176  std::cout << "No solution found" << std::endl;
177 }
178 
179 int main(int /*argc*/, char ** /*argv*/)
180 {
181  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
182  planWithSimpleSetup();
183  std::cout << std::endl;
184  return 0;
185 }
Definition of an abstract control.
Definition: Control.h:111
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:159
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
A TriangularDecomposition is a triangulation that ignores obstacles.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
ompl::control::Control ControlType
Define the type of control allocated by this control space.
Definition: ControlSpace.h:131
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:126
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 class to store the exit status of Planner::solve()
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:119
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:76
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.
Space information containing necessary information for planning with controls. setup() needs to be ca...
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:160
Definition of a scoped state.
Definition: ScopedState.h:120
TriangularDecomposition(const base::RealVectorBounds &bounds, std::vector< Polygon > holes=std::vector< Polygon >(), std::vector< Polygon > intRegs=std::vector< Polygon >())
Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional....
The lower and upper bounds for an Rn space.