demos/TriangulationDemo.cpp
00001 #include <ompl/control/SpaceInformation.h>
00002 #include <ompl/base/goals/GoalState.h>
00003 #include <ompl/base/spaces/SE2StateSpace.h>
00004 #include <ompl/control/spaces/RealVectorControlSpace.h>
00005 #include <ompl/control/planners/kpiece/KPIECE1.h>
00006 #include <ompl/control/planners/rrt/RRT.h>
00007 #include <ompl/control/planners/est/EST.h>
00008 #include <ompl/control/planners/syclop/SyclopRRT.h>
00009 #include <ompl/control/planners/syclop/SyclopEST.h>
00010 #include <ompl/control/SimpleSetup.h>
00011 #include <ompl/config.h>
00012 #include <ompl/extensions/triangle/TriangularDecomposition.h>
00013 #include <iostream>
00014 
00015 namespace ob = ompl::base;
00016 namespace oc = ompl::control;
00017 
00018 // a decomposition is only needed for SyclopRRT and SyclopEST
00019 class MyTriangularDecomposition : public oc::TriangularDecomposition
00020 {
00021 public:
00022     MyTriangularDecomposition(const ob::RealVectorBounds& bounds)
00023         : oc::TriangularDecomposition(bounds, createObstacles())
00024     {
00025         setup();
00026     }
00027     virtual void project(const ob::State* s, std::vector<double>& coord) const
00028     {
00029         coord.resize(2);
00030         coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
00031         coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
00032     }
00033 
00034     virtual void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const
00035     {
00036         sampler->sampleUniform(s);
00037         s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
00038     }
00039 
00040     std::vector<Polygon> createObstacles()
00041     {
00042         std::vector<Polygon> obst;
00043         Triangle tri;
00044         tri.pts[0].x = -0.5;
00045         tri.pts[0].y = 0.75;
00046         tri.pts[1].x = -0.75;
00047         tri.pts[1].y = 0.68;
00048         tri.pts[2].x = -0.5;
00049         tri.pts[2].y = 0.5;
00050         obst.push_back(tri);
00051 
00052         Polygon rect(4);
00053         rect.pts[0].x = 0.;
00054         rect.pts[0].y = 0.5;
00055         rect.pts[1].x = -0.3;
00056         rect.pts[1].y = 0.;
00057         rect.pts[2].x = 0.;
00058         rect.pts[2].y = -0.5;
00059         rect.pts[3].x = 0.6;
00060         rect.pts[3].y = 0.6;
00061         obst.push_back(rect);
00062 
00063         return obst;
00064     }
00065 };
00066 
00067 bool triContains(double x, double y, double ax, double ay, double bx, double by, double cx, double cy)
00068 {
00069     if ((x-ax)*(by-ay) - (bx-ax)*(y-ay) > 0.)
00070         return false;
00071     if ((x-bx)*(cy-by) - (cx-bx)*(y-by) > 0.)
00072         return false;
00073     if ((x-cx)*(ay-cy) - (ax-cx)*(y-cy) > 0.)
00074         return false;
00075     return true;
00076 }
00077 
00078 
00079 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
00080 {
00081     //    ob::ScopedState<ob::SE2StateSpace>
00082     // cast the abstract state type to the type we expect
00083     const ob::SE2StateSpace::StateType *se2state = state->as<ob::SE2StateSpace::StateType>();
00084 
00085     // check validity of state defined by pos & rot
00086     double x = se2state->getX();
00087     double y = se2state->getY();
00088     return si->satisfiesBounds(state) && !triContains(x,y, -0.5,0.75,-0.75,0.68,-0.5,0.5)
00089         && !triContains(x,y, 0,0.5,-0.3,0,0,-0.5)
00090         && !triContains(x,y,0,-0.5,0.6,0.6,0,0.5);
00091 }
00092 
00093 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
00094 {
00095     const ob::SE2StateSpace::StateType *se2state = start->as<ob::SE2StateSpace::StateType>();
00096     const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
00097     const ob::SO2StateSpace::StateType *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
00098     const oc::RealVectorControlSpace::ControlType *rctrl = control->as<oc::RealVectorControlSpace::ControlType>();
00099 
00100     result->as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[0] =
00101         (*pos)[0] + (*rctrl)[0] * duration * cos(rot->value);
00102 
00103     result->as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[1] =
00104         (*pos)[1] + (*rctrl)[0] * duration * sin(rot->value);
00105 
00106     result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1)->value =
00107         rot->value + (*rctrl)[1];
00108 }
00109 
00110 
00111 void planWithSimpleSetup(void)
00112 {
00113     // construct the state space we are planning in
00114     ob::StateSpacePtr space(new ob::SE2StateSpace());
00115 
00116     // set the bounds for the R^2 part of SE(2)
00117     ob::RealVectorBounds bounds(2);
00118     bounds.setLow(-1);
00119     bounds.setHigh(1);
00120 
00121     space->as<ob::SE2StateSpace>()->setBounds(bounds);
00122 
00123     // create a control space
00124     oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
00125 
00126     // set the bounds for the control space
00127     ob::RealVectorBounds cbounds(2);
00128     cbounds.setLow(-0.3);
00129     cbounds.setHigh(0.3);
00130 
00131     cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
00132 
00133     // define a simple setup class
00134     oc::SimpleSetup ss(cspace);
00135 
00136     // set the state propagation routine
00137     ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
00138 
00139     // set state validity checking for this space
00140     ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
00141 
00142     // create a start state
00143     ob::ScopedState<ob::SE2StateSpace> start(space);
00144     start->setX(-0.5);
00145     start->setY(0.0);
00146     start->setYaw(0.0);
00147 
00148     ob::ScopedState<ob::SE2StateSpace> goal(start);
00149     goal->setX(0.5);
00150 
00151     // set the start and goal states
00152     ss.setStartAndGoalStates(start, goal, 0.05);
00153 
00154     oc::TriangularDecomposition* td = new MyTriangularDecomposition(bounds);
00155     // print the triangulation to stdout
00156     td->print(std::cout);
00157 
00158     // hand the triangulation to SyclopEST
00159     ob::PlannerPtr planner(new oc::SyclopEST(ss.getSpaceInformation(), oc::DecompositionPtr(td)));
00160     // hand the SyclopEST planner to SimpleSetup
00161     ss.setPlanner(planner);
00162 
00163     // attempt to solve the problem within ten seconds of planning time
00164     ob::PlannerStatus solved = ss.solve(10.0);
00165 
00166     if (solved)
00167     {
00168         std::cout << "Found solution:" << std::endl;
00169         // print the path to screen
00170 
00171         ss.getSolutionPath().asGeometric().print(std::cout);
00172     }
00173     else
00174         std::cout << "No solution found" << std::endl;
00175 }
00176 
00177 int main(int, char **)
00178 {
00179     std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
00180     planWithSimpleSetup();
00181     std::cout << std::endl;
00182     return 0;
00183 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines