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 }