00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ompl/control/SpaceInformation.h>
00038 #include <ompl/base/GoalState.h>
00039 #include <ompl/base/spaces/SE2StateSpace.h>
00040 #include <ompl/control/spaces/RealVectorControlSpace.h>
00041 #include <ompl/control/planners/kpiece/KPIECE1.h>
00042 #include <ompl/control/planners/rrt/RRT.h>
00043 #include <ompl/control/planners/est/EST.h>
00044 #include <ompl/control/planners/syclop/SyclopRRT.h>
00045 #include <ompl/control/planners/syclop/SyclopEST.h>
00046 #include <ompl/control/SimpleSetup.h>
00047 #include <ompl/config.h>
00048 #include <iostream>
00049
00050 namespace ob = ompl::base;
00051 namespace oc = ompl::control;
00052
00053
00054 class MyDecomposition : public oc::GridDecomposition
00055 {
00056 public:
00057 MyDecomposition(const int length, const ob::RealVectorBounds& bounds)
00058 : GridDecomposition(length, 2, bounds)
00059 {
00060 }
00061 virtual void project(const ob::State* s, std::vector<double>& coord) const
00062 {
00063 coord.resize(2);
00064 coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
00065 coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
00066 }
00067 virtual void sampleFromRegion(const int rid, const ob::StateSamplerPtr& sampler, ob::State* s)
00068 {
00069 const ob::RealVectorBounds& regionBounds(getRegionBounds(rid));
00070 sampler->sampleUniform(s);
00071 s->as<ob::SE2StateSpace::StateType>()->setX(
00072 rng_.uniformReal(regionBounds.low[0], regionBounds.high[0]));
00073 s->as<ob::SE2StateSpace::StateType>()->setY(
00074 rng_.uniformReal(regionBounds.low[1], regionBounds.high[1]));
00075 }
00076
00077 private:
00078 ompl::RNG rng_;
00079 };
00080
00081
00082 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
00083 {
00084
00085
00086 const ob::SE2StateSpace::StateType *se2state = state->as<ob::SE2StateSpace::StateType>();
00087
00088
00089 const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
00090
00091
00092 const ob::SO2StateSpace::StateType *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
00093
00094
00095
00096
00097
00098 return si->satisfiesBounds(state) && (void*)rot != (void*)pos;
00099 }
00100
00101 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
00102 {
00103 const ob::SE2StateSpace::StateType *se2state = start->as<ob::SE2StateSpace::StateType>();
00104 const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
00105 const ob::SO2StateSpace::StateType *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
00106 const oc::RealVectorControlSpace::ControlType *rctrl = control->as<oc::RealVectorControlSpace::ControlType>();
00107
00108 result->as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[0] =
00109 (*pos)[0] + (*rctrl)[0] * duration * cos(rot->value);
00110
00111 result->as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[1] =
00112 (*pos)[1] + (*rctrl)[0] * duration * sin(rot->value);
00113
00114 result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1)->value =
00115 rot->value + (*rctrl)[1];
00116 }
00117
00118 void plan(void)
00119 {
00120
00121
00122 ob::StateSpacePtr space(new ob::SE2StateSpace());
00123
00124
00125 ob::RealVectorBounds bounds(2);
00126 bounds.setLow(-1);
00127 bounds.setHigh(1);
00128
00129 space->as<ob::SE2StateSpace>()->setBounds(bounds);
00130
00131
00132 oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
00133
00134
00135 ob::RealVectorBounds cbounds(2);
00136 cbounds.setLow(-0.3);
00137 cbounds.setHigh(0.3);
00138
00139 cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
00140
00141
00142 oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
00143
00144
00145 si->setStateValidityChecker(boost::bind(&isStateValid, si.get(), _1));
00146
00147
00148 si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
00149
00150
00151 ob::ScopedState<ob::SE2StateSpace> start(space);
00152 start->setX(-0.5);
00153 start->setY(0.0);
00154 start->setYaw(0.0);
00155
00156
00157 ob::ScopedState<ob::SE2StateSpace> goal(start);
00158 goal->setX(0.5);
00159
00160
00161 ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
00162
00163
00164 pdef->setStartAndGoalStates(start, goal, 0.1);
00165
00166
00167
00168
00169
00170 oc::DecompositionPtr decomp(new MyDecomposition(32, bounds));
00171 ob::PlannerPtr planner(new oc::SyclopEST(si, decomp));
00172
00173
00174
00175 planner->setProblemDefinition(pdef);
00176
00177
00178 planner->setup();
00179
00180
00181
00182 si->printSettings(std::cout);
00183
00184
00185 pdef->print(std::cout);
00186
00187
00188 bool solved = planner->solve(10.0);
00189
00190 if (solved)
00191 {
00192
00193
00194 ob::PathPtr path = pdef->getGoal()->getSolutionPath();
00195 std::cout << "Found solution:" << std::endl;
00196
00197
00198 path->print(std::cout);
00199 }
00200 else
00201 std::cout << "No solution found" << std::endl;
00202 }
00203
00204
00205 void planWithSimpleSetup(void)
00206 {
00207
00208 ob::StateSpacePtr space(new ob::SE2StateSpace());
00209
00210
00211 ob::RealVectorBounds bounds(2);
00212 bounds.setLow(-1);
00213 bounds.setHigh(1);
00214
00215 space->as<ob::SE2StateSpace>()->setBounds(bounds);
00216
00217
00218 oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
00219
00220
00221 ob::RealVectorBounds cbounds(2);
00222 cbounds.setLow(-0.3);
00223 cbounds.setHigh(0.3);
00224
00225 cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
00226
00227
00228 oc::SimpleSetup ss(cspace);
00229
00230
00231 ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
00232
00233
00234 ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
00235
00236
00237 ob::ScopedState<ob::SE2StateSpace> start(space);
00238 start->setX(-0.5);
00239 start->setY(0.0);
00240 start->setYaw(0.0);
00241
00242
00243 ob::ScopedState<ob::SE2StateSpace> goal(space);
00244 (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
00245 (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
00246 (*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;
00247
00248
00249
00250 ss.setStartAndGoalStates(start, goal, 0.05);
00251
00252
00253 bool solved = ss.solve(10.0);
00254
00255 if (solved)
00256 {
00257 std::cout << "Found solution:" << std::endl;
00258
00259
00260 ss.getSolutionPath().asGeometric().print(std::cout);
00261 }
00262 else
00263 std::cout << "No solution found" << std::endl;
00264 }
00265
00266 int main(int, char **)
00267 {
00268 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
00269
00270 plan();
00271
00272 std::cout << std::endl << std::endl;
00273
00274 planWithSimpleSetup();
00275
00276 return 0;
00277 }