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"
50 #include "ompl/util/ClassForward.h"
52 #include <boost/numeric/odeint.hpp>
53 #include <boost/function.hpp>
64 OMPL_CLASS_FORWARD(ODESolver);
82 typedef boost::function<void(const StateType &, const Control*, StateType &)>
ODE;
86 typedef boost::function<void(const base::State *state, const Control* control, const double duration, base::State *result)>
PostPropagationEvent;
137 OMPL_ERROR(
"ODESolverPtr does not reference a valid ODESolver object");
143 si_->getStateSpace()->copyToReals(reals, state);
144 solver_->solve (reals, control, duration);
145 si_->getStateSpace()->copyFromReals(result, reals);
148 postEvent_ (state, control, duration, result);
155 return StatePropagatorPtr(dynamic_cast<StatePropagator*>(
new ODESolverStatePropagator(solver, postEvent)));
176 ODEFunctor (
const ODE &o,
const Control* ctrl) : ode(o), control(ctrl) {}
181 ode (current, control, output);
196 template <
class Solver = boost::numeric::ode
int::runge_kutta4<ODESolver::StateType> >
213 ODESolver::ODEFunctor odefunc (
ode_, control);
214 boost::numeric::odeint::integrate_const (solver, odefunc, state, 0.0, duration,
intStep_);
224 template <
class Solver = boost::numeric::ode
int::runge_kutta_cash_karp54<ODESolver::StateType> >
245 ODESolver::ODEFunctor odefunc (
ode_, control);
247 if (
error_.size () != state.size ())
248 error_.assign (state.size (), 0.0);
251 solver.adjust_size (state);
254 while (time < duration)
271 template <
class Solver = boost::numeric::ode
int::runge_kutta_cash_karp54<ODESolver::StateType> >
313 ODESolver::ODEFunctor odefunc (
ode_, control);
315 boost::numeric::odeint::controlled_runge_kutta< Solver > solver (boost::numeric::odeint::default_error_checker<double>(
maxError_,
maxEpsilonError_));
316 boost::numeric::odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration,
intStep_);