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 typedef oc::PropositionalTriangularDecomposition::Polygon Polygon;
56 typedef oc::PropositionalTriangularDecomposition::Vertex 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  virtual ~MyDecomposition() { }
66 
67  virtual void project(const ob::State* s, std::vector<double>& coord) const
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  virtual void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const
75  {
76  sampler->sampleUniform(s);
78  ws->setXY(coord[0], coord[1]);
79  }
80 
81 private:
82  ompl::RNG rng_;
83 };
84 
85 void addObstaclesAndPropositions(oc::PropositionalTriangularDecomposition* decomp)
86 {
87  Polygon obstacle(4);
88  obstacle.pts[0] = Vertex(0.,.9);
89  obstacle.pts[1] = Vertex(1.1,.9);
90  obstacle.pts[2] = Vertex(1.1,1.1);
91  obstacle.pts[3] = Vertex(0.,1.1);
92  decomp->addHole(obstacle);
93 
94  Polygon p0(4);
95  p0.pts[0] = Vertex(.9,.3);
96  p0.pts[1] = Vertex(1.1,.3);
97  p0.pts[2] = Vertex(1.1,.5);
98  p0.pts[3] = Vertex(.9,.5);
99  decomp->addProposition(p0);
100 
101  Polygon p1(4);
102  p1.pts[0] = Vertex(1.5,1.6);
103  p1.pts[1] = Vertex(1.6,1.6);
104  p1.pts[2] = Vertex(1.6,1.7);
105  p1.pts[3] = Vertex(1.5,1.7);
106  decomp->addProposition(p1);
107 
108  Polygon p2(4);
109  p2.pts[0] = Vertex(.2,1.7);
110  p2.pts[1] = Vertex(.3,1.7);
111  p2.pts[2] = Vertex(.3,1.8);
112  p2.pts[3] = Vertex(.2,1.8);
113  decomp->addProposition(p2);
114 }
115 
116 /* Returns whether a point (x,y) is within a given polygon.
117  We are assuming that the polygon is a axis-aligned rectangle, with vertices stored
118  in counter-clockwise order, beginning with the bottom-left vertex. */
119 bool polyContains(const Polygon& poly, double x, double y)
120 {
121  return x >= poly.pts[0].x && x <= poly.pts[2].x
122  && y >= poly.pts[0].y && y <= poly.pts[2].y;
123 }
124 
125 /* Our state validity checker queries the decomposition for its obstacles,
126  and checks for collisions against them.
127  This is to prevent us from having to redefine the obstacles in multiple places. */
128 bool isStateValid(
129  const oc::SpaceInformation *si,
131  const ob::State *state)
132 {
133  if (!si->satisfiesBounds(state))
134  return false;
136 
137  double x = se2->getX();
138  double y = se2->getY();
139  const std::vector<Polygon>& obstacles = decomp->getHoles();
140  typedef std::vector<Polygon>::const_iterator ObstacleIter;
141  for (ObstacleIter o = obstacles.begin(); o != obstacles.end(); ++o)
142  {
143  if (polyContains(*o, x, y))
144  return false;
145  }
146  return true;
147 }
148 
149 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
150 {
152  const oc::RealVectorControlSpace::ControlType* rctrl = control->as<oc::RealVectorControlSpace::ControlType>();
153 
154  double xout = se2->getX() + rctrl->values[0]*duration*cos(se2->getYaw());
155  double yout = se2->getY() + rctrl->values[0]*duration*sin(se2->getYaw());
156  double yawout = se2->getYaw() + rctrl->values[1];
157 
159  se2out->setXY(xout, yout);
160  se2out->setYaw(yawout);
161 
163  ob::SO2StateSpace SO2;
164  SO2.enforceBounds (so2out);
165 }
166 
167 void plan(void)
168 {
169  // construct the state space we are planning in
171 
172  // set the bounds for the R^2 part of SE(2)
173  ob::RealVectorBounds bounds(2);
174  bounds.setLow(0);
175  bounds.setHigh(2);
176 
177  space->as<ob::SE2StateSpace>()->setBounds(bounds);
178 
179  // create triangulation that ignores obstacle and respects propositions
180  MyDecomposition* ptd = new MyDecomposition(bounds);
181  // helper method that adds an obstacle, as well as three propositions p0,p1,p2
182  addObstaclesAndPropositions(ptd);
183  ptd->setup();
185 
186  // create a control space
187  oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
188 
189  // set the bounds for the control space
190  ob::RealVectorBounds cbounds(2);
191  cbounds.setLow(-.5);
192  cbounds.setHigh(.5);
193 
194  cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
195 
196  oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
197  si->setStateValidityChecker(boost::bind(&isStateValid, si.get(), ptd, _1));
198  si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
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  oc::ProductGraphPtr product(new oc::ProductGraph(pd, 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  oc::LTLSpaceInformationPtr ltlsi(new oc::LTLSpaceInformation(si, product));
224 
225  // LTLProblemDefinition creates a goal in hybrid space, corresponding to any
226  // state in which both automata are accepting
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 = new oc::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->as<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  delete ltlPlanner;
261 }
262 
263 int main(int, char **)
264 {
265  plan();
266  return 0;
267 }
A planner for generating system trajectories to satisfy a logical specification given by an automaton...
Definition: LTLPlanner.h:58
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:72
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:48
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost 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...
A boost shared pointer wrapper for ompl::control::PropositionalDecomposition.
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
A ProductGraph represents the weighted, directed, graph-based Cartesian product of a PropositionalDec...
Definition: ProductGraph.h:67
State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
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:226
A boost shared pointer wrapper for ompl::control::ControlSpace.
A boost shared pointer wrapper for ompl::control::LTLSpaceInformation.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
virtual void sampleFullState(const base::StateSamplerPtr &sampler, const std::vector< double > &coord, base::State *s) const
Samples a State using a projected coordinate and a StateSampler.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
A control space representing Rn.
Base class for a planner.
Definition: Planner.h:232
A state space representing SE(2)
Definition: SE2StateSpace.h:50
A boost shared pointer wrapper for ompl::control::LTLProblemDefinition.
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
virtual void project(const base::State *s, std::vector< double > &coord) const
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition...
virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: Planner.cpp:75
Definition of an abstract state.
Definition: State.h:50
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
A boost shared pointer wrapper for ompl::control::SpaceInformation.
A boost shared pointer wrapper for ompl::control::ProductGraph.
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 boost 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:65
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
virtual void enforceBounds(State *state) const
Normalize the value of the state to the interval (-Pi, Pi].
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
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:266
Space information containing necessary information for planning with controls. setup() needs to be ca...
T * as()
Cast this instance to a desired type.
Definition: Planner.h:247