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);
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;
133 
134  double x = se2->getX();
135  double y = se2->getY();
136  const std::vector<Polygon>& obstacles = decomp->getHoles();
137  using ObstacleIter = std::vector<Polygon>::const_iterator;
138  for (const auto & obstacle : obstacles)
139  {
140  if (polyContains(obstacle, x, y))
141  return false;
142  }
143  return true;
144 }
145 
146 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
147 {
149  const oc::RealVectorControlSpace::ControlType* rctrl = control->as<oc::RealVectorControlSpace::ControlType>();
150 
151  double xout = se2->getX() + rctrl->values[0]*duration*cos(se2->getYaw());
152  double yout = se2->getY() + rctrl->values[0]*duration*sin(se2->getYaw());
153  double yawout = se2->getYaw() + rctrl->values[1];
154 
156  se2out->setXY(xout, yout);
157  se2out->setYaw(yawout);
158 
160  ob::SO2StateSpace SO2;
161  SO2.enforceBounds (so2out);
162 }
163 
164 void plan()
165 {
166  // construct the state space we are planning in
167  auto space(std::make_shared<ob::SE2StateSpace>());
168 
169  // set the bounds for the R^2 part of SE(2)
170  ob::RealVectorBounds bounds(2);
171  bounds.setLow(0);
172  bounds.setHigh(2);
173 
174  space->setBounds(bounds);
175 
176  // create triangulation that ignores obstacle and respects propositions
177  std::shared_ptr<oc::PropositionalTriangularDecomposition> ptd = std::make_shared<MyDecomposition>(bounds);
178  // helper method that adds an obstacle, as well as three propositions p0,p1,p2
179  addObstaclesAndPropositions(ptd);
180  ptd->setup();
181 
182  // create a control space
183  auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
184 
185  // set the bounds for the control space
186  ob::RealVectorBounds cbounds(2);
187  cbounds.setLow(-.5);
188  cbounds.setHigh(.5);
189 
190  cspace->setBounds(cbounds);
191 
192  oc::SpaceInformationPtr si(std::make_shared<oc::SpaceInformation>(space, cspace));
194  [&si, ptd](const ob::State *state)
195  {
196  return isStateValid(si.get(), ptd, state);
197  });
198  si->setStatePropagator(propagate);
199  si->setPropagationStepSize(0.025);
200 
201  //LTL co-safety sequencing formula: visit p2,p0 in that order
202  std::vector<unsigned int> sequence(2);
203  sequence[0] = 2;
204  sequence[1] = 0;
205  oc::AutomatonPtr cosafety = oc::Automaton::SequenceAutomaton(3, sequence);
206 
207  //LTL safety avoidance formula: never visit p1
208  std::vector<unsigned int> toAvoid(1);
209  toAvoid[0] = 1;
211 
212  //construct product graph (propDecomp x A_{cosafety} x A_{safety})
213  auto product(std::make_shared<oc::ProductGraph>(ptd, cosafety, safety));
214 
215  // LTLSpaceInformation creates a hybrid space of robot state space x product graph.
216  // It takes the validity checker from SpaceInformation and expands it to one that also
217  // rejects any hybrid state containing rejecting automaton states.
218  // It takes the state propagator from SpaceInformation and expands it to one that
219  // follows continuous propagation with setting the next decomposition region
220  // and automaton states accordingly.
221  //
222  // The robot state space, given by SpaceInformation, is referred to as the "lower space".
223  auto ltlsi(std::make_shared<oc::LTLSpaceInformation>(si, product));
224 
225  // LTLProblemDefinition creates a goal in hybrid space, corresponding to any
226  // state in which both automata are accepting
227  auto pdef(std::make_shared<oc::LTLProblemDefinition>(ltlsi));
228 
229  // create a start state
231  start->setX(0.2);
232  start->setY(0.2);
233  start->setYaw(0.0);
234 
235  // addLowerStartState accepts a state in lower space, expands it to its
236  // corresponding hybrid state (decomposition region containing the state, and
237  // starting states in both automata), and adds that as an official start state.
238  pdef->addLowerStartState(start.get());
239 
240  //LTL planner (input: LTL space information, product automaton)
241  oc::LTLPlanner ltlPlanner(ltlsi, product);
242  ltlPlanner.setProblemDefinition(pdef);
243 
244  // attempt to solve the problem within thirty seconds of planning time
245  // considering the above cosafety/safety automata, a solution path is any
246  // path that visits p2 followed by p0 while avoiding obstacles and avoiding p1.
247  ob::PlannerStatus solved = ltlPlanner.ob::Planner::solve(30.0);
248 
249  if (solved)
250  {
251  std::cout << "Found solution:" << std::endl;
252  // The path returned by LTLProblemDefinition is through hybrid space.
253  // getLowerSolutionPath() projects it down into the original robot state space
254  // that we handed to LTLSpaceInformation.
255  pdef->getLowerSolutionPath()->print(std::cout);
256  }
257  else
258  std::cout << "No solution found" << std::endl;
259 }
260 
261 int main(int, char **)
262 {
263  plan();
264  return 0;
265 }
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:58
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...
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:47
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
A shared pointer wrapper for ompl::base::StateSampler.
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
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:253
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
A PropositionalTriangularDecomposition is a triangulation that ignores obstacles and respects proposi...
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
A shared pointer wrapper for ompl::control::SpaceInformation.
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
void setPropagationStepSize(double stepSize)
When controls are applied to states, they are applied for a time duration that is an integer multiple...
The lower and upper bounds for an Rn space.
A shared pointer wrapper for ompl::control::Automaton.
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...
A state space representing SO(2). The distance function and interpolation take into account angle wra...
Definition: SO2StateSpace.h:63
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
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:295
Space information containing necessary information for planning with controls. setup() needs to be ca...
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.