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)
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
00158 struct ODEFunctor
00159 {
00160 ODEFunctor (const ODE &o, const Control* ctrl) : ode(o), control(ctrl) {}
00161
00162
00163 void operator () (const StateType ¤t, StateType &output, double )
00164 {
00165 ode (current, control, output);
00166 }
00167
00168 ODE ode;
00169 const Control* control;
00170 };
00172 };
00173
00180 template <class Solver = boost::numeric::omplext_odeint::runge_kutta4<ODESolver::StateType> >
00181 class ODEBasicSolver : public ODESolver
00182 {
00183 public:
00184
00187 ODEBasicSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
00188 {
00189 }
00190
00191 protected:
00192
00194 virtual void solve (StateType &state, const Control* control, const double duration) const
00195 {
00196 Solver solver;
00197 ODESolver::ODEFunctor odefunc (ode_, control);
00198 boost::numeric::omplext_odeint::integrate_const (solver, odefunc, state, 0.0, duration, intStep_);
00199 }
00200 };
00201
00208 template <class Solver = boost::numeric::omplext_odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
00209 class ODEErrorSolver : public ODESolver
00210 {
00211 public:
00214 ODEErrorSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
00215 {
00216 }
00217
00219 ODESolver::StateType getError (void)
00220 {
00221 ODESolver::StateType error (error_.begin (), error_.end ());
00222 return error;
00223 }
00224
00225 protected:
00227 virtual void solve (StateType &state, const Control* control, const double duration) const
00228 {
00229 ODESolver::ODEFunctor odefunc (ode_, control);
00230
00231 if (error_.size () != state.size ())
00232 error_.assign (state.size (), 0.0);
00233
00234 Solver solver;
00235 solver.adjust_size (state);
00236
00237 double time = 0.0;
00238 while (time < duration)
00239 {
00240 solver.do_step (odefunc, state, time, intStep_, error_);
00241 time += intStep_;
00242 }
00243 }
00244
00246 mutable ODESolver::StateType error_;
00247 };
00248
00255 template <class Solver = boost::numeric::omplext_odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
00256 class ODEAdaptiveSolver : public ODESolver
00257 {
00258 public:
00261 ODEAdaptiveSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
00262 {
00263 }
00264
00266 double getMaximumError (void) const
00267 {
00268 return maxError_;
00269 }
00270
00272 void setMaximumError (double error)
00273 {
00274 maxError_ = error;
00275 }
00276
00278 double getMaximumEpsilonError (void) const
00279 {
00280 return maxEpsilonError_;
00281 }
00282
00284 void setMaximumEpsilonError (double error)
00285 {
00286 maxEpsilonError_ = error;
00287 }
00288
00289 protected:
00290
00295 virtual void solve (StateType &state, const Control* control, const double duration) const
00296 {
00297 ODESolver::ODEFunctor odefunc (ode_, control);
00298
00299 boost::numeric::omplext_odeint::controlled_runge_kutta< Solver > solver (boost::numeric::omplext_odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
00300 boost::numeric::omplext_odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration, intStep_);
00301 }
00302
00304 double maxError_;
00305
00307 double maxEpsilonError_;
00308 };
00309 }
00310 }
00311
00312 #endif
00313
00314 #endif