Loading...
Searching...
No Matches
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
15namespace ob = ompl::base;
16namespace oc = ompl::control;
17
18// a decomposition is only needed for SyclopRRT and SyclopEST
19class MyTriangularDecomposition : public oc::TriangularDecomposition
20{
21public:
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
67bool 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
79bool 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
93void 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
111void 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();
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
179int 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}
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition State.h:95
The lower and upper bounds for an Rn space.
A state in SE(2): (x, y, yaw)
The definition of a state in SO(2)
Definition of a scoped state.
Definition ScopedState.h:57
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition StateSpace.h:78
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
Definition of an abstract control.
Definition Control.h:48
const T * as() const
Cast this instance to a desired type.
Definition Control.h:64
Create the set of classes typically needed to solve a control problem.
Definition SimpleSetup.h:63
Space information containing necessary information for planning with controls. setup() needs to be ca...
A TriangularDecomposition is a triangulation that ignores obstacles.
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....
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition Time.h:55
A class to store the exit status of Planner::solve()