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 #ifndef OMPL_CONTROL_ODESOLVER_
00038 #define OMPL_CONTROL_ODESOLVER_
00039
00040
00041 #include <boost/version.hpp>
00042 #if BOOST_VERSION < 104400
00043 #warning Boost version >=1.44 is needed for ODESolver classes
00044 #else
00045
00046 #include "ompl/control/Control.h"
00047 #include "ompl/control/SpaceInformation.h"
00048 #include "ompl/control/StatePropagator.h"
00049 #include "ompl/util/Console.h"
00050
00051 #include <omplext_odeint/boost/numeric/odeint.hpp>
00052 #include <boost/function.hpp>
00053 #include <cassert>
00054 #include <vector>
00055
00056 namespace ompl
00057 {
00058
00059 namespace control
00060 {
00061
00066 class ODESolver
00067 {
00068 public:
00070 typedef std::vector<double> StateType;
00071
00074 typedef boost::function<void(const StateType &, const Control*, StateType &)> ODE;
00075
00078 typedef boost::function<void(const Control*, base::State*)> PostPropagationEvent;
00079
00082 ODESolver (const SpaceInformationPtr &si, const ODE &ode, double intStep) : si_(si), ode_(ode), intStep_(intStep), msg_("ODESolver")
00083 {
00084 }
00085
00087 virtual ~ODESolver (void)
00088 {
00089 }
00090
00092 void setODE (const ODE &ode)
00093 {
00094 ode_ = ode;
00095 }
00096
00098 double getIntegrationStepSize (void) const
00099 {
00100 return intStep_;
00101 }
00102
00104 void setIntegrationStepSize (double intStep)
00105 {
00106 intStep_ = intStep;
00107 }
00108
00114 StatePropagatorPtr getStatePropagator (const PostPropagationEvent &postEvent = NULL) const
00115 {
00116 class ODESolverStatePropagator : public StatePropagator
00117 {
00118 public:
00119 ODESolverStatePropagator (const SpaceInformationPtr& si, const ODESolver *solver, const PostPropagationEvent &pe) : StatePropagator (si), solver_(solver), postEvent_(pe)
00120 {
00121 }
00122
00123 virtual void propagate (const base::State *state, const Control* control, const double duration, base::State *result) const
00124 {
00125 ODESolver::StateType reals;
00126 si_->getStateSpace()->copyToReals(reals, state);
00127 solver_->solve (reals, control, duration);
00128 si_->getStateSpace()->copyFromReals(result, reals);
00129
00130 if (postEvent_)
00131 postEvent_ (control, result);
00132 }
00133
00134 protected:
00135 const ODESolver *solver_;
00136 ODESolver::PostPropagationEvent postEvent_;
00137 };
00138
00139 return StatePropagatorPtr(dynamic_cast<StatePropagator*>(new ODESolverStatePropagator(si_, this, postEvent)));
00140 }
00141
00142 protected:
00143
00145 virtual void solve (StateType &state, const Control* control, const double duration) const = 0;
00146
00148 const SpaceInformationPtr si_;
00149
00151 ODE ode_;
00152
00154 double intStep_;
00155
00157 msg::Interface msg_;
00158
00160
00161 struct ODEFunctor
00162 {
00163 ODEFunctor (const ODE &o, const Control* ctrl) : ode(o), control(ctrl) {}
00164
00165
00166 void operator () (const StateType ¤t, StateType &output, double )
00167 {
00168 ode (current, control, output);
00169 }
00170
00171 ODE ode;
00172 const Control* control;
00173 };
00175 };
00176
00183 template <class Solver = boost::numeric::omplext_odeint::runge_kutta4<ODESolver::StateType> >
00184 class ODEBasicSolver : public ODESolver
00185 {
00186 public:
00187
00190 ODEBasicSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
00191 {
00192 }
00193
00194 protected:
00195
00197 virtual void solve (StateType &state, const Control* control, const double duration) const
00198 {
00199 Solver solver;
00200 ODESolver::ODEFunctor odefunc (ode_, control);
00201 boost::numeric::omplext_odeint::integrate_const (solver, odefunc, state, 0.0, duration, intStep_);
00202 }
00203 };
00204
00211 template <class Solver = boost::numeric::omplext_odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
00212 class ODEErrorSolver : public ODESolver
00213 {
00214 public:
00217 ODEErrorSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
00218 {
00219 }
00220
00222 ODESolver::StateType getError (void)
00223 {
00224 ODESolver::StateType error (error_.begin (), error_.end ());
00225 return error;
00226 }
00227
00228 protected:
00230 virtual void solve (StateType &state, const Control* control, const double duration) const
00231 {
00232 ODESolver::ODEFunctor odefunc (ode_, control);
00233
00234 if (error_.size () != state.size ())
00235 error_.assign (state.size (), 0.0);
00236
00237 Solver solver;
00238 solver.adjust_size (state);
00239
00240 double time = 0.0;
00241 while (time < duration)
00242 {
00243 solver.do_step (odefunc, state, time, intStep_, error_);
00244 time += intStep_;
00245 }
00246 }
00247
00249 mutable ODESolver::StateType error_;
00250 };
00251
00258 template <class Solver = boost::numeric::omplext_odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
00259 class ODEAdaptiveSolver : public ODESolver
00260 {
00261 public:
00264 ODEAdaptiveSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
00265 {
00266 }
00267
00269 double getMaximumError (void) const
00270 {
00271 return maxError_;
00272 }
00273
00275 void setMaximumError (double error)
00276 {
00277 maxError_ = error;
00278 }
00279
00281 double getMaximumEpsilonError (void) const
00282 {
00283 return maxEpsilonError_;
00284 }
00285
00287 void setMaximumEpsilonError (double error)
00288 {
00289 maxEpsilonError_ = error;
00290 }
00291
00292 protected:
00293
00298 virtual void solve (StateType &state, const Control* control, const double duration) const
00299 {
00300 ODESolver::ODEFunctor odefunc (ode_, control);
00301
00302 boost::numeric::omplext_odeint::controlled_runge_kutta< Solver > solver (boost::numeric::omplext_odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
00303 boost::numeric::omplext_odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration, intStep_);
00304 }
00305
00307 double maxError_;
00308
00310 double maxEpsilonError_;
00311 };
00312 }
00313 }
00314
00315 #endif
00316
00317 #endif