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 #if BOOST_VERSION >= 105300
53 #include <boost/numeric/odeint.hpp>
54 namespace odeint = boost::numeric::odeint;
56 #include <omplext_odeint/boost/numeric/odeint.hpp>
57 namespace odeint = boost::numeric::omplext_odeint;
59 #include <boost/function.hpp>
70 OMPL_CLASS_FORWARD(ODESolver);
88 typedef boost::function<void(const StateType &, const Control*, StateType &)>
ODE;
92 typedef boost::function<void(const base::State *state, const Control* control, const double duration, base::State *result)>
PostPropagationEvent;
143 OMPL_ERROR(
"ODESolverPtr does not reference a valid ODESolver object");
149 si_->getStateSpace()->copyToReals(reals, state);
150 solver_->solve (reals, control, duration);
151 si_->getStateSpace()->copyFromReals(result, reals);
154 postEvent_ (state, control, duration, result);
161 return StatePropagatorPtr(dynamic_cast<StatePropagator*>(
new ODESolverStatePropagator(solver, postEvent)));
182 ODEFunctor (
const ODE &o,
const Control* ctrl) : ode(o), control(ctrl) {}
187 ode (current, control, output);
202 template <
class Solver = ode
int::runge_kutta4<ODESolver::StateType> >
219 ODESolver::ODEFunctor odefunc (
ode_, control);
220 odeint::integrate_const (solver, odefunc, state, 0.0, duration,
intStep_);
230 template <
class Solver = ode
int::runge_kutta_cash_karp54<ODESolver::StateType> >
251 ODESolver::ODEFunctor odefunc (
ode_, control);
253 if (
error_.size () != state.size ())
254 error_.assign (state.size (), 0.0);
257 solver.adjust_size (state);
260 while (time < duration)
277 template <
class Solver = ode
int::runge_kutta_cash_karp54<ODESolver::StateType> >
319 ODESolver::ODEFunctor odefunc (
ode_, control);
322 odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration,
intStep_);
Solver for ordinary differential equations of the type q' = f(q, u), where q is the current state of ...
ODEErrorSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve, and the integration step size - default is 0.01.
ODE ode_
Definition of the ODE to find solutions for.
Definition of an abstract control.
double getMaximumEpsilonError(void) const
Retrieve the error tolerance during one step of numerical integration (local truncation error) ...
static StatePropagatorPtr getStatePropagator(ODESolverPtr solver, const PostPropagationEvent &postEvent=NULL)
Retrieve a StatePropagator object that solves a system of ordinary differential equations defined by ...
ODEAdaptiveSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve, and an optional integration step size - default is 0.01.
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
boost::function< void(const StateType &, const Control *, StateType &)> ODE
Callback function that defines the ODE. Accepts the current state, input control, and output state...
void setODE(const ODE &ode)
Set the ODE to solve.
A boost shared pointer wrapper for ompl::control::ODESolver.
Model the effect of controls on system states.
void setMaximumError(double error)
Set the total error allowed during numerical integration.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
virtual void solve(StateType &state, const Control *control, const double duration) const
Solve the ODE using boost::numeric::odeint. Save the resulting error values into error_.
virtual void solve(StateType &state, const Control *control, const double duration) const =0
Solve the ODE given the initial state, and a control to apply for some duration.
std::vector< double > StateType
Portable data type for the state values.
virtual ~ODESolver(void)
Destructor.
Adaptive step size solver for ordinary differential equations of the type q' = f(q, u), where q is the current state of the system and u is a control applied to the system. The maximum integration error is bounded in this approach. Solver is the numerical integration method used to solve the equations, and must implement the error stepper concept from boost::numeric::odeint. The default is a fifth order Runge-Kutta Cash-Karp method with a fourth order error bound.
const SpaceInformationPtr si_
The SpaceInformation that this ODESolver operates in.
virtual void solve(StateType &state, const Control *control, const double duration) const
Solve the ODE using boost::numeric::odeint.
Definition of an abstract state.
void setIntegrationStepSize(double intStep)
Set the size of a single numerical integration step.
boost::function< void(const base::State *state, const Control *control, const double duration, base::State *result)> PostPropagationEvent
Callback function to perform an event at the end of numerical integration. This functionality is opti...
ODESolver::StateType error_
The error values calculated during numerical integration.
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
void setMaximumEpsilonError(double error)
Set the error tolerance during one step of numerical integration (local truncation error) ...
double maxError_
The maximum error allowed when performing numerical integration.
Abstract base class for an object that can solve ordinary differential equations (ODE) of the type q'...
ODEBasicSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve, and an optional integration step size - default is 0.01.
ODESolver(const SpaceInformationPtr &si, const ODE &ode, double intStep)
Parameterized constructor. Takes a reference to SpaceInformation, an ODE to solve, and the integration step size.
virtual void solve(StateType &state, const Control *control, const double duration) const
Solve the ordinary differential equation given the input state of the system, a control to apply to t...
ODESolver::StateType getError(void)
Retrieves the error values from the most recent integration.
double intStep_
The size of the numerical integration step. Should be small to minimize error.
double maxEpsilonError_
The maximum error allowed during one step of numerical integration.
double getMaximumError(void) const
Retrieve the total error allowed during numerical integration.
double getIntegrationStepSize(void) const
Return the size of a single numerical integration step.