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/SimpleSetup.h>
00044 #include <ompl/config.h>
00045 #include <iostream>
00046 #include <valarray>
00047 #include <limits>
00048
00049 namespace ob = ompl::base;
00050 namespace oc = ompl::control;
00051
00052
00054 class KinematicCarModel
00055 {
00056 public:
00057
00058 KinematicCarModel(const ob::StateSpace *space) : space_(space), carLength_(0.2)
00059 {
00060 }
00061
00063 void operator()(const ob::State *state, const oc::Control *control, std::valarray<double> &dstate) const
00064 {
00065 const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
00066 const double theta = state->as<ob::SE2StateSpace::StateType>()->getYaw();
00067
00068 dstate.resize(3);
00069 dstate[0] = u[0] * cos(theta);
00070 dstate[1] = u[0] * sin(theta);
00071 dstate[2] = u[0] * tan(u[1]) / carLength_;
00072 }
00073
00075 void update(ob::State *state, const std::valarray<double> &dstate) const
00076 {
00077 ob::SE2StateSpace::StateType &s = *state->as<ob::SE2StateSpace::StateType>();
00078 s.setX(s.getX() + dstate[0]);
00079 s.setY(s.getY() + dstate[1]);
00080 s.setYaw(s.getYaw() + dstate[2]);
00081 space_->enforceBounds(state);
00082 }
00083
00084 private:
00085
00086 const ob::StateSpace *space_;
00087 const double carLength_;
00088
00089 };
00090
00091
00093 template<typename F>
00094 class EulerIntegrator
00095 {
00096 public:
00097
00098 EulerIntegrator(const ob::StateSpace *space, double timeStep) : space_(space), timeStep_(timeStep), ode_(space)
00099 {
00100 }
00101
00102 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
00103 {
00104 double t = timeStep_;
00105 std::valarray<double> dstate;
00106 space_->copyState(result, start);
00107 while (t < duration + std::numeric_limits<double>::epsilon())
00108 {
00109 ode_(result, control, dstate);
00110 ode_.update(result, timeStep_ * dstate);
00111 t += timeStep_;
00112 }
00113 if (t + std::numeric_limits<double>::epsilon() > duration)
00114 {
00115 ode_(result, control, dstate);
00116 ode_.update(result, (t - duration) * dstate);
00117 }
00118 }
00119
00120 double getTimeStep(void) const
00121 {
00122 return timeStep_;
00123 }
00124
00125 void setTimeStep(double timeStep)
00126 {
00127 timeStep_ = timeStep;
00128 }
00129
00130 private:
00131
00132 const ob::StateSpace *space_;
00133 double timeStep_;
00134 F ode_;
00135 };
00136
00137
00138 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
00139 {
00140
00142 const ob::SE2StateSpace::StateType *se2state = state->as<ob::SE2StateSpace::StateType>();
00143
00145 const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
00146
00148 const ob::SO2StateSpace::StateType *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
00149
00151
00152
00153
00154 return si->satisfiesBounds(state) && (void*)rot != (void*)pos;
00155 }
00156
00158 class DemoControlSpace : public oc::RealVectorControlSpace
00159 {
00160 public:
00161
00162 DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
00163 {
00164 }
00165 };
00166
00167 class DemoStatePropagator : public oc::StatePropagator
00168 {
00169 public:
00170
00171 DemoStatePropagator(const oc::SpaceInformationPtr &si) : oc::StatePropagator(si),
00172 integrator_(si->getStateSpace().get(), 0.0)
00173 {
00174 }
00175
00176 virtual void propagate(const ob::State *state, const oc::Control* control, const double duration, ob::State *result) const
00177 {
00178 integrator_.propagate(state, control, duration, result);
00179 }
00180
00181 void setIntegrationTimeStep(double timeStep)
00182 {
00183 integrator_.setTimeStep(timeStep);
00184 }
00185
00186 double getIntegrationTimeStep(void) const
00187 {
00188 return integrator_.getTimeStep();
00189 }
00190
00191 EulerIntegrator<KinematicCarModel> integrator_;
00192 };
00193
00195
00196 void planWithSimpleSetup(void)
00197 {
00199 ob::StateSpacePtr space(new ob::SE2StateSpace());
00200
00202 ob::RealVectorBounds bounds(2);
00203 bounds.setLow(-1);
00204 bounds.setHigh(1);
00205
00206 space->as<ob::SE2StateSpace>()->setBounds(bounds);
00207
00208
00209 oc::ControlSpacePtr cspace(new DemoControlSpace(space));
00210
00211
00212 ob::RealVectorBounds cbounds(2);
00213 cbounds.setLow(-0.3);
00214 cbounds.setHigh(0.3);
00215
00216 cspace->as<DemoControlSpace>()->setBounds(cbounds);
00217
00218
00219 oc::SimpleSetup ss(cspace);
00220
00222 ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
00223
00225 ss.setStatePropagator(oc::StatePropagatorPtr(new DemoStatePropagator(ss.getSpaceInformation())));
00226
00228 ob::ScopedState<ob::SE2StateSpace> start(space);
00229 start->setX(-0.5);
00230 start->setY(0.0);
00231 start->setYaw(0.0);
00232
00234 ob::ScopedState<ob::SE2StateSpace> goal(space);
00235 goal->setX(0.0);
00236 goal->setY(0.5);
00237 goal->setYaw(0.0);
00238
00240 ss.setStartAndGoalStates(start, goal, 0.05);
00241
00243 ss.setup();
00244 static_cast<DemoStatePropagator*>(ss.getStatePropagator().get())->setIntegrationTimeStep(ss.getSpaceInformation()->getPropagationStepSize());
00245
00247 bool solved = ss.solve(10.0);
00248
00249 if (solved)
00250 {
00251 std::cout << "Found solution:" << std::endl;
00253
00254 ss.getSolutionPath().asGeometric().print(std::cout);
00255 }
00256 else
00257 std::cout << "No solution found" << std::endl;
00258 }
00259
00260 int main(int, char **)
00261 {
00262 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
00263
00264 planWithSimpleSetup();
00265
00266 return 0;
00267 }