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 
52 namespace ob = ompl::base;
53 namespace oc = ompl::control;
54 
55 using Polygon = oc::PropositionalTriangularDecomposition::Polygon;
56 using Vertex = oc::PropositionalTriangularDecomposition::Vertex;
57 
58 // a decomposition is only needed for SyclopRRT and SyclopEST
59 // use TriangularDecomp
60 class MyDecomposition : public oc::PropositionalTriangularDecomposition
61 {
62 public:
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 
82 void 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. */
116 bool 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. */
125 bool 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 
145 void 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);
159  ob::SO2StateSpace SO2;
160  SO2.enforceBounds (so2out);
161 }
162 
163 void 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 
264 int main(int /*argc*/, char ** /*argv*/)
265 {
266  plan();
267  return 0;
268 }
Definition of an abstract control.
Definition: Control.h:111
A state space representing SO(2). The distance function and interpolation take into account angle wra...
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...
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....
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
A PropositionalTriangularDecomposition is a triangulation that ignores obstacles and respects proposi...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
static AutomatonPtr AvoidanceAutomaton(unsigned int numProps, const std::vector< unsigned int > &avoidProps)
Returns an avoidance automaton, which rejects when any one of the given list of propositions becomes ...
Definition: Automaton.cpp:382
ompl::control::Control ControlType
Define the type of control allocated by this control space.
Definition: ControlSpace.h:131
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.
A planner for generating system trajectories to satisfy a logical specification given by an automaton...
Definition: LTLPlanner.h:122
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
Space information containing necessary information for planning with controls. setup() needs to be ca...
Definition of a control path.
Definition: PathControl.h:92
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:160
Definition of a scoped state.
Definition: ScopedState.h:120
static AutomatonPtr SequenceAutomaton(unsigned int numProps, const std::vector< unsigned int > &seqProps)
Helper function to return a sequence automaton. Assumes all propositions are mutually exclusive.
Definition: Automaton.cpp:340
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.
The lower and upper bounds for an Rn space.