demos/LTLWithTriangulation.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Matt Maly */ 00036 00037 #include <ompl/control/SpaceInformation.h> 00038 #include <ompl/base/spaces/SE2StateSpace.h> 00039 #include <ompl/control/spaces/RealVectorControlSpace.h> 00040 #include <ompl/control/SimpleSetup.h> 00041 #include <ompl/config.h> 00042 #include <iostream> 00043 #include <vector> 00044 00045 #include <ompl/extensions/triangle/PropositionalTriangularDecomposition.h> 00046 #include <ompl/control/planners/ltl/PropositionalDecomposition.h> 00047 #include <ompl/control/planners/ltl/Automaton.h> 00048 #include <ompl/control/planners/ltl/ProductGraph.h> 00049 #include <ompl/control/planners/ltl/LTLPlanner.h> 00050 #include <ompl/control/planners/ltl/LTLProblemDefinition.h> 00051 00052 namespace ob = ompl::base; 00053 namespace oc = ompl::control; 00054 00055 typedef oc::PropositionalTriangularDecomposition::Polygon Polygon; 00056 typedef oc::PropositionalTriangularDecomposition::Vertex Vertex; 00057 00058 // a decomposition is only needed for SyclopRRT and SyclopEST 00059 // use TriangularDecomp 00060 class MyDecomposition : public oc::PropositionalTriangularDecomposition 00061 { 00062 public: 00063 MyDecomposition(const ob::RealVectorBounds& bounds) 00064 : oc::PropositionalTriangularDecomposition(bounds) { } 00065 virtual ~MyDecomposition() { } 00066 00067 virtual void project(const ob::State* s, std::vector<double>& coord) const 00068 { 00069 coord.resize(2); 00070 coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX(); 00071 coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY(); 00072 } 00073 00074 virtual void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const 00075 { 00076 sampler->sampleUniform(s); 00077 ob::SE2StateSpace::StateType* ws = s->as<ob::SE2StateSpace::StateType>(); 00078 ws->setXY(coord[0], coord[1]); 00079 } 00080 00081 private: 00082 ompl::RNG rng_; 00083 }; 00084 00085 void addObstaclesAndPropositions(oc::PropositionalTriangularDecomposition* decomp) 00086 { 00087 Polygon obstacle(4); 00088 obstacle.pts[0] = Vertex(0.,.9); 00089 obstacle.pts[1] = Vertex(1.1,.9); 00090 obstacle.pts[2] = Vertex(1.1,1.1); 00091 obstacle.pts[3] = Vertex(0.,1.1); 00092 decomp->addHole(obstacle); 00093 00094 Polygon p0(4); 00095 p0.pts[0] = Vertex(.9,.3); 00096 p0.pts[1] = Vertex(1.1,.3); 00097 p0.pts[2] = Vertex(1.1,.5); 00098 p0.pts[3] = Vertex(.9,.5); 00099 decomp->addProposition(p0); 00100 00101 Polygon p1(4); 00102 p1.pts[0] = Vertex(1.5,1.6); 00103 p1.pts[1] = Vertex(1.6,1.6); 00104 p1.pts[2] = Vertex(1.6,1.7); 00105 p1.pts[3] = Vertex(1.5,1.7); 00106 decomp->addProposition(p1); 00107 00108 Polygon p2(4); 00109 p2.pts[0] = Vertex(.2,1.7); 00110 p2.pts[1] = Vertex(.3,1.7); 00111 p2.pts[2] = Vertex(.3,1.8); 00112 p2.pts[3] = Vertex(.2,1.8); 00113 decomp->addProposition(p2); 00114 } 00115 00116 /* Returns whether a point (x,y) is within a given polygon. 00117 We are assuming that the polygon is a axis-aligned rectangle, with vertices stored 00118 in counter-clockwise order, beginning with the bottom-left vertex. */ 00119 bool polyContains(const Polygon& poly, double x, double y) 00120 { 00121 return x >= poly.pts[0].x && x <= poly.pts[2].x 00122 && y >= poly.pts[0].y && y <= poly.pts[2].y; 00123 } 00124 00125 /* Our state validity checker queries the decomposition for its obstacles, 00126 and checks for collisions against them. 00127 This is to prevent us from having to redefine the obstacles in multiple places. */ 00128 bool isStateValid( 00129 const oc::SpaceInformation *si, 00130 const oc::PropositionalTriangularDecomposition* decomp, 00131 const ob::State *state) 00132 { 00133 if (!si->satisfiesBounds(state)) 00134 return false; 00135 const ob::SE2StateSpace::StateType* se2 = state->as<ob::SE2StateSpace::StateType>(); 00136 00137 double x = se2->getX(); 00138 double y = se2->getY(); 00139 const std::vector<Polygon>& obstacles = decomp->getHoles(); 00140 typedef std::vector<Polygon>::const_iterator ObstacleIter; 00141 for (ObstacleIter o = obstacles.begin(); o != obstacles.end(); ++o) 00142 { 00143 if (polyContains(*o, x, y)) 00144 return false; 00145 } 00146 return true; 00147 } 00148 00149 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) 00150 { 00151 const ob::SE2StateSpace::StateType* se2 = start->as<ob::SE2StateSpace::StateType>(); 00152 const oc::RealVectorControlSpace::ControlType* rctrl = control->as<oc::RealVectorControlSpace::ControlType>(); 00153 00154 double xout = se2->getX() + rctrl->values[0]*duration*cos(se2->getYaw()); 00155 double yout = se2->getY() + rctrl->values[0]*duration*sin(se2->getYaw()); 00156 double yawout = se2->getYaw() + rctrl->values[1]; 00157 00158 ob::SE2StateSpace::StateType* se2out = result->as<ob::SE2StateSpace::StateType>(); 00159 se2out->setXY(xout, yout); 00160 se2out->setYaw(yawout); 00161 00162 ob::SO2StateSpace::StateType* so2out = se2out->as<ob::SO2StateSpace::StateType>(1); 00163 ob::SO2StateSpace SO2; 00164 SO2.enforceBounds (so2out); 00165 } 00166 00167 void plan(void) 00168 { 00169 // construct the state space we are planning in 00170 ob::StateSpacePtr space(new ob::SE2StateSpace()); 00171 00172 // set the bounds for the R^2 part of SE(2) 00173 ob::RealVectorBounds bounds(2); 00174 bounds.setLow(0); 00175 bounds.setHigh(2); 00176 00177 space->as<ob::SE2StateSpace>()->setBounds(bounds); 00178 00179 // create triangulation that ignores obstacle and respects propositions 00180 MyDecomposition* ptd = new MyDecomposition(bounds); 00181 // helper method that adds an obstacle, as well as three propositions p0,p1,p2 00182 addObstaclesAndPropositions(ptd); 00183 ptd->setup(); 00184 oc::PropositionalDecompositionPtr pd(ptd); 00185 00186 // create a control space 00187 oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2)); 00188 00189 // set the bounds for the control space 00190 ob::RealVectorBounds cbounds(2); 00191 cbounds.setLow(-.5); 00192 cbounds.setHigh(.5); 00193 00194 cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds); 00195 00196 oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace)); 00197 si->setStateValidityChecker(boost::bind(&isStateValid, si.get(), ptd, _1)); 00198 si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4)); 00199 si->setPropagationStepSize(0.025); 00200 00201 //LTL co-safety sequencing formula: visit p2,p0 in that order 00202 std::vector<unsigned int> sequence(2); 00203 sequence[0] = 2; 00204 sequence[1] = 0; 00205 oc::AutomatonPtr cosafety = oc::Automaton::SequenceAutomaton(3, sequence); 00206 00207 //LTL safety avoidance formula: never visit p1 00208 std::vector<unsigned int> toAvoid(1); 00209 toAvoid[0] = 1; 00210 oc::AutomatonPtr safety = oc::Automaton::AvoidanceAutomaton(3, toAvoid); 00211 00212 //construct product graph (propDecomp x A_{cosafety} x A_{safety}) 00213 oc::ProductGraphPtr product(new oc::ProductGraph(pd, cosafety, safety)); 00214 00215 // LTLSpaceInformation creates a hybrid space of robot state space x product graph. 00216 // It takes the validity checker from SpaceInformation and expands it to one that also 00217 // rejects any hybrid state containing rejecting automaton states. 00218 // It takes the state propagator from SpaceInformation and expands it to one that 00219 // follows continuous propagation with setting the next decomposition region 00220 // and automaton states accordingly. 00221 // 00222 // The robot state space, given by SpaceInformation, is referred to as the "lower space". 00223 oc::LTLSpaceInformationPtr ltlsi(new oc::LTLSpaceInformation(si, product)); 00224 00225 // LTLProblemDefinition creates a goal in hybrid space, corresponding to any 00226 // state in which both automata are accepting 00227 oc::LTLProblemDefinitionPtr pdef(new oc::LTLProblemDefinition(ltlsi)); 00228 00229 // create a start state 00230 ob::ScopedState<ob::SE2StateSpace> start(space); 00231 start->setX(0.2); 00232 start->setY(0.2); 00233 start->setYaw(0.0); 00234 00235 // addLowerStartState accepts a state in lower space, expands it to its 00236 // corresponding hybrid state (decomposition region containing the state, and 00237 // starting states in both automata), and adds that as an official start state. 00238 pdef->addLowerStartState(start.get()); 00239 00240 //LTL planner (input: LTL space information, product automaton) 00241 oc::LTLPlanner* ltlPlanner = new oc::LTLPlanner(ltlsi, product); 00242 ltlPlanner->setProblemDefinition(pdef); 00243 00244 // attempt to solve the problem within thirty seconds of planning time 00245 // considering the above cosafety/safety automata, a solution path is any 00246 // path that visits p2 followed by p0 while avoiding obstacles and avoiding p1. 00247 ob::PlannerStatus solved = ltlPlanner->as<ob::Planner>()->solve(30.0); 00248 00249 if (solved) 00250 { 00251 std::cout << "Found solution:" << std::endl; 00252 // The path returned by LTLProblemDefinition is through hybrid space. 00253 // getLowerSolutionPath() projects it down into the original robot state space 00254 // that we handed to LTLSpaceInformation. 00255 pdef->getLowerSolutionPath()->print(std::cout); 00256 } 00257 else 00258 std::cout << "No solution found" << std::endl; 00259 00260 delete ltlPlanner; 00261 } 00262 00263 int main(int, char **) 00264 { 00265 plan(); 00266 return 0; 00267 }