37 #ifndef OMPL_CONTROL_ODESOLVER_
38 #define OMPL_CONTROL_ODESOLVER_
41 #include <boost/version.hpp>
42 #if BOOST_VERSION < 104400
43 #warning Boost version >=1.44 is needed for ODESolver classes
46 #include "ompl/control/Control.h"
47 #include "ompl/control/SpaceInformation.h"
48 #include "ompl/control/StatePropagator.h"
51 #include <omplext_odeint/boost/numeric/odeint.hpp>
52 #include <boost/function.hpp>
74 typedef boost::function<void(const StateType &, const Control*, StateType &)>
ODE;
126 si_->getStateSpace()->copyToReals(reals, state);
127 solver_->solve (reals, control, duration);
128 si_->getStateSpace()->copyFromReals(result, reals);
131 postEvent_ (control, result);
139 return StatePropagatorPtr(dynamic_cast<StatePropagator*>(
new ODESolverStatePropagator(
si_,
this, postEvent)));
160 ODEFunctor (
const ODE &o,
const Control* ctrl) : ode(o), control(ctrl) {}
165 ode (current, control, output);
180 template <
class Solver = boost::numeric::omplext_ode
int::runge_kutta4<ODESolver::StateType> >
197 ODESolver::ODEFunctor odefunc (
ode_, control);
198 boost::numeric::omplext_odeint::integrate_const (solver, odefunc, state, 0.0, duration,
intStep_);
208 template <
class Solver = boost::numeric::omplext_ode
int::runge_kutta_cash_karp54<ODESolver::StateType> >
229 ODESolver::ODEFunctor odefunc (
ode_, control);
231 if (
error_.size () != state.size ())
232 error_.assign (state.size (), 0.0);
235 solver.adjust_size (state);
238 while (time < duration)
255 template <
class Solver = boost::numeric::omplext_ode
int::runge_kutta_cash_karp54<ODESolver::StateType> >
297 ODESolver::ODEFunctor odefunc (
ode_, control);
299 boost::numeric::omplext_odeint::controlled_runge_kutta< Solver > solver (boost::numeric::omplext_odeint::default_error_checker<double>(
maxError_,
maxEpsilonError_));
300 boost::numeric::omplext_odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration,
intStep_);