Loading...
Searching...
No Matches
LTLWithTriangulation.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2013, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Matt Maly */
36
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>
42#include <iostream>
43#include <vector>
44
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>
51
52namespace ob = ompl::base;
53namespace oc = ompl::control;
54
55using Polygon = oc::PropositionalTriangularDecomposition::Polygon;
56using Vertex = oc::PropositionalTriangularDecomposition::Vertex;
57
58// a decomposition is only needed for SyclopRRT and SyclopEST
59// use TriangularDecomp
60class MyDecomposition : public oc::PropositionalTriangularDecomposition
61{
62public:
63 MyDecomposition(const ob::RealVectorBounds& bounds)
65 ~MyDecomposition() override = default;
66
67 void project(const ob::State* s, std::vector<double>& coord) const override
68 {
69 coord.resize(2);
70 coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
71 coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
72 }
73
74 void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const override
75 {
76 sampler->sampleUniform(s);
77 auto* ws = s->as<ob::SE2StateSpace::StateType>();
78 ws->setXY(coord[0], coord[1]);
79 }
80};
81
82void addObstaclesAndPropositions(std::shared_ptr<oc::PropositionalTriangularDecomposition> &decomp)
83{
84 Polygon obstacle(4);
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);
90
91 Polygon p0(4);
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);
97
98 Polygon p1(4);
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);
104
105 Polygon p2(4);
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);
111}
112
113/* Returns whether a point (x,y) is within a given polygon.
114 We are assuming that the polygon is a axis-aligned rectangle, with vertices stored
115 in counter-clockwise order, beginning with the bottom-left vertex. */
116bool polyContains(const Polygon& poly, double x, double y)
117{
118 return x >= poly.pts[0].x && x <= poly.pts[2].x
119 && y >= poly.pts[0].y && y <= poly.pts[2].y;
120}
121
122/* Our state validity checker queries the decomposition for its obstacles,
123 and checks for collisions against them.
124 This is to prevent us from having to redefine the obstacles in multiple places. */
125bool isStateValid(
126 const oc::SpaceInformation *si,
127 const std::shared_ptr<oc::PropositionalTriangularDecomposition> &decomp,
128 const ob::State *state)
129{
130 if (!si->satisfiesBounds(state))
131 return false;
132 const auto* se2 = state->as<ob::SE2StateSpace::StateType>();
133
134 double x = se2->getX();
135 double y = se2->getY();
136 const std::vector<Polygon>& obstacles = decomp->getHoles();
137 for (const auto & obstacle : obstacles)
138 {
139 if (polyContains(obstacle, x, y))
140 return false;
141 }
142 return true;
143}
144
145void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
146{
147 const auto* se2 = start->as<ob::SE2StateSpace::StateType>();
148 const auto* rctrl = control->as<oc::RealVectorControlSpace::ControlType>();
149
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];
153
154 auto* se2out = result->as<ob::SE2StateSpace::StateType>();
155 se2out->setXY(xout, yout);
156 se2out->setYaw(yawout);
157
158 auto* so2out = se2out->as<ob::SO2StateSpace::StateType>(1);
160 SO2.enforceBounds (so2out);
161}
162
163void plan()
164{
165 // construct the state space we are planning in
166 auto space(std::make_shared<ob::SE2StateSpace>());
167
168 // set the bounds for the R^2 part of SE(2)
169 ob::RealVectorBounds bounds(2);
170 bounds.setLow(0);
171 bounds.setHigh(2);
172
173 space->setBounds(bounds);
174
175 // create triangulation that ignores obstacle and respects propositions
176 std::shared_ptr<oc::PropositionalTriangularDecomposition> ptd = std::make_shared<MyDecomposition>(bounds);
177 // helper method that adds an obstacle, as well as three propositions p0,p1,p2
178 addObstaclesAndPropositions(ptd);
179 ptd->setup();
180
181 // create a control space
182 auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
183
184 // set the bounds for the control space
185 ob::RealVectorBounds cbounds(2);
186 cbounds.setLow(-.5);
187 cbounds.setHigh(.5);
188
189 cspace->setBounds(cbounds);
190
191 oc::SpaceInformationPtr si(std::make_shared<oc::SpaceInformation>(space, cspace));
192 si->setStateValidityChecker(
193 [&si, ptd](const ob::State *state)
194 {
195 return isStateValid(si.get(), ptd, state);
196 });
197 si->setStatePropagator(propagate);
198 si->setPropagationStepSize(0.025);
199
200 //LTL co-safety sequencing formula: visit p2,p0 in that order
201#if OMPL_HAVE_SPOT
202 // This shows off the capability to construct an automaton from LTL-cosafe formula using Spot
203 auto cosafety = std::make_shared<oc::Automaton>(3, "! p0 U ((p2 & !p0) & XF p0)");
204#else
205 auto cosafety = oc::Automaton::SequenceAutomaton(3, {2, 0});
206#endif
207 //LTL safety avoidance formula: never visit p1
208#if OMPL_HAVE_SPOT
209 // This shows off the capability to construct an automaton from LTL-safe formula using Spot
210 auto safety = std::make_shared<oc::Automaton>(3, "G ! p1", false);
211#else
212 auto safety = oc::Automaton::AvoidanceAutomaton(3, {1});
213#endif
214
215 // construct product graph (propDecomp x A_{cosafety} x A_{safety})
216 auto product(std::make_shared<oc::ProductGraph>(ptd, cosafety, safety));
217
218 // LTLSpaceInformation creates a hybrid space of robot state space x product graph.
219 // It takes the validity checker from SpaceInformation and expands it to one that also
220 // rejects any hybrid state containing rejecting automaton states.
221 // It takes the state propagator from SpaceInformation and expands it to one that
222 // follows continuous propagation with setting the next decomposition region
223 // and automaton states accordingly.
224 //
225 // The robot state space, given by SpaceInformation, is referred to as the "lower space".
226 auto ltlsi(std::make_shared<oc::LTLSpaceInformation>(si, product));
227
228 // LTLProblemDefinition creates a goal in hybrid space, corresponding to any
229 // state in which both automata are accepting
230 auto pdef(std::make_shared<oc::LTLProblemDefinition>(ltlsi));
231
232 // create a start state
234 start->setX(0.2);
235 start->setY(0.2);
236 start->setYaw(0.0);
237
238 // addLowerStartState accepts a state in lower space, expands it to its
239 // corresponding hybrid state (decomposition region containing the state, and
240 // starting states in both automata), and adds that as an official start state.
241 pdef->addLowerStartState(start.get());
242
243 //LTL planner (input: LTL space information, product automaton)
244 oc::LTLPlanner ltlPlanner(ltlsi, product);
245 ltlPlanner.setProblemDefinition(pdef);
246
247 // attempt to solve the problem within thirty seconds of planning time
248 // considering the above cosafety/safety automata, a solution path is any
249 // path that visits p2 followed by p0 while avoiding obstacles and avoiding p1.
250 ob::PlannerStatus solved = ltlPlanner.ob::Planner::solve(30.0);
251
252 if (solved)
253 {
254 std::cout << "Found solution:" << std::endl;
255 // The path returned by LTLProblemDefinition is through hybrid space.
256 // getLowerSolutionPath() projects it down into the original robot state space
257 // that we handed to LTLSpaceInformation.
258 static_cast<oc::PathControl &>(*pdef->getLowerSolutionPath()).printAsMatrix(std::cout);
259 }
260 else
261 std::cout << "No solution found" << std::endl;
262}
263
264int main(int /*argc*/, char ** /*argv*/)
265{
266 plan();
267 return 0;
268}
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 ScopedState.h:57
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
A planner for generating system trajectories to satisfy a logical specification given by an automaton...
Definition LTLPlanner.h:59
Definition of a control path.
Definition PathControl.h:61
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path,...
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....
Space information containing necessary information for planning with controls. setup() needs to be ca...
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()