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/spaces/SE2StateSpace.h>
00039 #include <ompl/control/ODESolver.h>
00040 #include <ompl/control/spaces/RealVectorControlSpace.h>
00041 #include <ompl/control/SimpleSetup.h>
00042 #include <ompl/config.h>
00043 #include <iostream>
00044 #include <valarray>
00045 #include <limits>
00046
00047 namespace ob = ompl::base;
00048 namespace oc = ompl::control;
00049
00050
00051 class KinematicCarModel : public oc::StatePropagator
00052 {
00053 public:
00054 KinematicCarModel(const oc::SpaceInformationPtr &si) : oc::StatePropagator(si)
00055 {
00056 space_ = si->getStateSpace();
00057 carLength_ = 0.2;
00058 timeStep_ = 0.01;
00059 }
00060
00061 virtual void propagate(const ob::State *state, const oc::Control* control, const double duration, ob::State *result) const
00062 {
00063 EulerIntegration(state, control, duration, result);
00064 }
00065
00066 protected:
00067
00068 void EulerIntegration(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
00069 {
00070 double t = timeStep_;
00071 std::valarray<double> dstate;
00072 space_->copyState(result, start);
00073 while (t < duration + std::numeric_limits<double>::epsilon())
00074 {
00075 ode(result, control, dstate);
00076 update(result, timeStep_ * dstate);
00077 t += timeStep_;
00078 }
00079 if (t + std::numeric_limits<double>::epsilon() > duration)
00080 {
00081 ode(result, control, dstate);
00082 update(result, (t - duration) * dstate);
00083 }
00084 }
00085
00087 void ode(const ob::State *state, const oc::Control *control, std::valarray<double> &dstate) const
00088 {
00089 const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
00090 const double theta = state->as<ob::SE2StateSpace::StateType>()->getYaw();
00091
00092 dstate.resize(3);
00093 dstate[0] = u[0] * cos(theta);
00094 dstate[1] = u[0] * sin(theta);
00095 dstate[2] = u[0] * tan(u[1]) / carLength_;
00096 }
00097
00099 void update(ob::State *state, const std::valarray<double> &dstate) const
00100 {
00101 ob::SE2StateSpace::StateType &s = *state->as<ob::SE2StateSpace::StateType>();
00102 s.setX(s.getX() + dstate[0]);
00103 s.setY(s.getY() + dstate[1]);
00104 s.setYaw(s.getYaw() + dstate[2]);
00105 space_->enforceBounds(state);
00106 }
00107
00108 ob::StateSpacePtr space_;
00109 double carLength_;
00110 double timeStep_;
00111 };
00112
00113
00114
00115 void KinematicCarODE (const oc::ODESolver::StateType& q, const oc::Control* control, oc::ODESolver::StateType& qdot)
00116 {
00117 const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
00118 const double theta = q[2];
00119 double carLength = 0.2;
00120
00121
00122 qdot.resize (q.size (), 0);
00123
00124 qdot[0] = u[0] * cos(theta);
00125 qdot[1] = u[0] * sin(theta);
00126 qdot[2] = u[0] * tan(u[1]) / carLength;
00127 }
00128
00129
00130 void KinematicCarPostIntegration (const oc::Control* , ob::State* result)
00131 {
00132
00133 ob::SO2StateSpace SO2;
00134 SO2.enforceBounds (result->as<ob::SE2StateSpace::StateType>());
00135 }
00136
00137 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
00138 {
00139
00141 const ob::SE2StateSpace::StateType *se2state = state->as<ob::SE2StateSpace::StateType>();
00142
00144 const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
00145
00147 const ob::SO2StateSpace::StateType *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
00148
00150
00151
00152
00153 return si->satisfiesBounds(state) && (void*)rot != (void*)pos;
00154 }
00155
00157 class DemoControlSpace : public oc::RealVectorControlSpace
00158 {
00159 public:
00160
00161 DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
00162 {
00163 }
00164 };
00166
00167 void planWithSimpleSetup(void)
00168 {
00170 ob::StateSpacePtr space(new ob::SE2StateSpace());
00171
00173 ob::RealVectorBounds bounds(2);
00174 bounds.setLow(-1);
00175 bounds.setHigh(1);
00176
00177 space->as<ob::SE2StateSpace>()->setBounds(bounds);
00178
00179
00180 oc::ControlSpacePtr cspace(new DemoControlSpace(space));
00181
00182
00183 ob::RealVectorBounds cbounds(2);
00184 cbounds.setLow(-0.3);
00185 cbounds.setHigh(0.3);
00186
00187 cspace->as<DemoControlSpace>()->setBounds(cbounds);
00188
00189
00190 oc::SimpleSetup ss(cspace);
00191
00192
00193 ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
00194
00195
00196
00197
00198
00199
00200
00201 oc::ODEBasicSolver<> odeSolver(ss.getSpaceInformation(), &KinematicCarODE);
00202 ss.setStatePropagator(odeSolver.getStatePropagator(&KinematicCarPostIntegration));
00203
00205 ob::ScopedState<ob::SE2StateSpace> start(space);
00206 start->setX(-0.5);
00207 start->setY(0.0);
00208 start->setYaw(0.0);
00209
00211 ob::ScopedState<ob::SE2StateSpace> goal(space);
00212 goal->setX(0.0);
00213 goal->setY(0.5);
00214 goal->setYaw(0.0);
00215
00217 ss.setStartAndGoalStates(start, goal, 0.05);
00218
00220 ss.setup();
00221
00223 ob::PlannerStatus solved = ss.solve(10.0);
00224
00225 if (solved)
00226 {
00227 std::cout << "Found solution:" << std::endl;
00229
00230 ss.getSolutionPath().asGeometric().print(std::cout);
00231 }
00232 else
00233 std::cout << "No solution found" << std::endl;
00234 }
00235
00236 int main(int, char **)
00237 {
00238 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
00239
00240 planWithSimpleSetup();
00241
00242 return 0;
00243 }