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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines