37#include <ompl/control/SpaceInformation.h>
38#include <ompl/base/spaces/SE2StateSpace.h>
39#include <ompl/control/spaces/RealVectorControlSpace.h>
40#include <ompl/control/SimpleSetup.h>
41#include <ompl/config.h>
45#include <ompl/extensions/triangle/PropositionalTriangularDecomposition.h>
46#include <ompl/control/planners/ltl/PropositionalDecomposition.h>
47#include <ompl/control/planners/ltl/Automaton.h>
48#include <ompl/control/planners/ltl/ProductGraph.h>
49#include <ompl/control/planners/ltl/LTLPlanner.h>
50#include <ompl/control/planners/ltl/LTLProblemDefinition.h>
55using Polygon = oc::PropositionalTriangularDecomposition::Polygon;
56using Vertex = oc::PropositionalTriangularDecomposition::Vertex;
65 ~MyDecomposition()
override =
default;
70 coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
71 coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
74 void sampleFullState(
const ob::StateSamplerPtr& sampler,
const std::vector<double>& coord,
ob::State* s)
const override
76 sampler->sampleUniform(s);
77 auto* ws = s->as<ob::SE2StateSpace::StateType>();
78 ws->setXY(coord[0], coord[1]);
82void addObstaclesAndPropositions(std::shared_ptr<oc::PropositionalTriangularDecomposition> &decomp)
85 obstacle.pts[0] = Vertex(0.,.9);
86 obstacle.pts[1] = Vertex(1.1,.9);
87 obstacle.pts[2] = Vertex(1.1,1.1);
88 obstacle.pts[3] = Vertex(0.,1.1);
89 decomp->addHole(obstacle);
92 p0.pts[0] = Vertex(.9,.3);
93 p0.pts[1] = Vertex(1.1,.3);
94 p0.pts[2] = Vertex(1.1,.5);
95 p0.pts[3] = Vertex(.9,.5);
96 decomp->addProposition(p0);
99 p1.pts[0] = Vertex(1.5,1.6);
100 p1.pts[1] = Vertex(1.6,1.6);
101 p1.pts[2] = Vertex(1.6,1.7);
102 p1.pts[3] = Vertex(1.5,1.7);
103 decomp->addProposition(p1);
106 p2.pts[0] = Vertex(.2,1.7);
107 p2.pts[1] = Vertex(.3,1.7);
108 p2.pts[2] = Vertex(.3,1.8);
109 p2.pts[3] = Vertex(.2,1.8);
110 decomp->addProposition(p2);
116bool polyContains(
const Polygon& poly,
double x,
double y)
118 return x >= poly.pts[0].x && x <= poly.pts[2].x
119 && y >= poly.pts[0].y && y <= poly.pts[2].y;
127 const std::shared_ptr<oc::PropositionalTriangularDecomposition> &decomp,
130 if (!si->satisfiesBounds(state))
132 const auto* se2 = state->
as<ob::SE2StateSpace::StateType>();
134 double x = se2->getX();
135 double y = se2->getY();
136 const std::vector<Polygon>& obstacles = decomp->getHoles();
137 for (
const auto & obstacle : obstacles)
139 if (polyContains(obstacle, x, y))
147 const auto* se2 = start->
as<ob::SE2StateSpace::StateType>();
148 const auto* rctrl = control->
as<oc::RealVectorControlSpace::ControlType>();
150 double xout = se2->getX() + rctrl->values[0]*
duration*cos(se2->getYaw());
151 double yout = se2->getY() + rctrl->values[0]*
duration*sin(se2->getYaw());
152 double yawout = se2->getYaw() + rctrl->values[1];
154 auto* se2out = result->
as<ob::SE2StateSpace::StateType>();
155 se2out->setXY(xout, yout);
156 se2out->setYaw(yawout);
158 auto* so2out = se2out->as<ob::SO2StateSpace::StateType>(1);
166 auto space(std::make_shared<ob::SE2StateSpace>());
173 space->setBounds(bounds);
176 std::shared_ptr<oc::PropositionalTriangularDecomposition> ptd = std::make_shared<MyDecomposition>(bounds);
178 addObstaclesAndPropositions(ptd);
182 auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
189 cspace->setBounds(cbounds);
191 oc::SpaceInformationPtr si(std::make_shared<oc::SpaceInformation>(space, cspace));
192 si->setStateValidityChecker(
195 return isStateValid(si.get(), ptd, state);
197 si->setStatePropagator(propagate);
198 si->setPropagationStepSize(0.025);
203 auto cosafety = std::make_shared<oc::Automaton>(3,
"! p0 U ((p2 & !p0) & XF p0)");
205 auto cosafety = oc::Automaton::SequenceAutomaton(3, {2, 0});
210 auto safety = std::make_shared<oc::Automaton>(3,
"G ! p1",
false);
212 auto safety = oc::Automaton::AvoidanceAutomaton(3, {1});
216 auto product(std::make_shared<oc::ProductGraph>(ptd, cosafety, safety));
226 auto ltlsi(std::make_shared<oc::LTLSpaceInformation>(si, product));
230 auto pdef(std::make_shared<oc::LTLProblemDefinition>(ltlsi));
241 pdef->addLowerStartState(start.get());
245 ltlPlanner.setProblemDefinition(pdef);
254 std::cout <<
"Found solution:" << std::endl;
258 static_cast<oc::PathControl &
>(*pdef->getLowerSolutionPath()).printAsMatrix(std::cout);
261 std::cout <<
"No solution found" << std::endl;
264int main(
int ,
char ** )
The lower and upper bounds for an Rn space.
A state space representing SO(2). The distance function and interpolation take into account angle wra...
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
Definition of a scoped state.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
Definition of an abstract control.
const T * as() const
Cast this instance to a desired type.
A planner for generating system trajectories to satisfy a logical specification given by an automaton...
Definition of a control path.
void sampleFullState(const base::StateSamplerPtr &sampler, const std::vector< double > &coord, base::State *s) const override
Samples a State using a projected coordinate and a StateSampler.
void project(const base::State *s, std::vector< double > &coord) const override
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition.
A PropositionalTriangularDecomposition is a triangulation that ignores obstacles and respects proposi...
PropositionalTriangularDecomposition(const base::RealVectorBounds &bounds, const std::vector< Polygon > &holes=std::vector< Polygon >(), const std::vector< Polygon > &props=std::vector< Polygon >())
Creates a PropositionalTriangularDecomposition 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...
std::chrono::system_clock::duration duration
Representation of a time duration.
A class to store the exit status of Planner::solve()