ODESolver.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ryan Luna */
36 
37 #ifndef OMPL_CONTROL_ODESOLVER_
38 #define OMPL_CONTROL_ODESOLVER_
39 
40 #include "ompl/control/Control.h"
41 #include "ompl/control/SpaceInformation.h"
42 #include "ompl/control/StatePropagator.h"
43 #include "ompl/util/Console.h"
44 #include "ompl/util/ClassForward.h"
45 
46 #include <boost/version.hpp>
47 #include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
48 #include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
49 #include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
50 #include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
51 #include <boost/numeric/odeint/integrate/integrate_const.hpp>
52 #include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
53 namespace odeint = boost::numeric::odeint;
54 #include <functional>
55 #include <cassert>
56 #include <utility>
57 #include <vector>
58 
59 namespace ompl
60 {
61  namespace control
62  {
64  OMPL_CLASS_FORWARD(ODESolver);
66 
69 
74  class ODESolver
75  {
76  public:
78  typedef std::vector<double> StateType;
79 
82  typedef std::function<void(const StateType &, const Control *, StateType &)> ODE;
83 
86  typedef std::function<void(const base::State *, const Control *, double, base::State *)>
88 
91  ODESolver(SpaceInformationPtr si, ODE ode, double intStep)
92  : si_(std::move(si)), ode_(std::move(ode)), intStep_(intStep)
93  {
94  }
95 
97  virtual ~ODESolver() = default;
98 
100  void setODE(const ODE &ode)
101  {
102  ode_ = ode;
103  }
104 
106  double getIntegrationStepSize() const
107  {
108  return intStep_;
109  }
110 
112  void setIntegrationStepSize(double intStep)
113  {
114  intStep_ = intStep;
115  }
116 
119  {
120  return si_;
121  }
122 
128  static StatePropagatorPtr getStatePropagator(ODESolverPtr solver,
129  const PostPropagationEvent &postEvent = nullptr)
130  {
131  class ODESolverStatePropagator : public StatePropagator
132  {
133  public:
134  ODESolverStatePropagator(const ODESolverPtr& solver, PostPropagationEvent pe)
135  : StatePropagator(solver->si_), solver_(solver), postEvent_(std::move(pe))
136  {
137  if (solver == nullptr)
138  OMPL_ERROR("ODESolverPtr does not reference a valid ODESolver object");
139  }
140 
141  void propagate(const base::State *state, const Control *control, double duration,
142  base::State *result) const override
143  {
144  ODESolver::StateType reals;
145  si_->getStateSpace()->copyToReals(reals, state);
146  solver_->solve(reals, control, duration);
147  si_->getStateSpace()->copyFromReals(result, reals);
148 
149  if (postEvent_)
150  postEvent_(state, control, duration, result);
151  }
152 
153  protected:
154  ODESolverPtr solver_;
156  };
157  return std::make_shared<ODESolverStatePropagator>(std::move(solver), postEvent);
158  }
159 
160  protected:
162  virtual void solve(StateType &state, const Control *control, double duration) const = 0;
163 
166 
169 
171  double intStep_;
172 
174  // Functor used by the boost::numeric::odeint stepper object
175  struct ODEFunctor
176  {
177  ODEFunctor(ODE o, const Control *ctrl) : ode(std::move(o)), control(ctrl)
178  {
179  }
180 
181  // boost::numeric::odeint will callback to this method during integration to evaluate the system
182  void operator()(const StateType &current, StateType &output, double /*time*/)
183  {
184  ode(current, control, output);
185  }
186 
187  ODE ode;
188  const Control *control;
189  };
191  };
192 
199  template <class Solver = odeint::runge_kutta4<ODESolver::StateType>>
200  class ODEBasicSolver : public ODESolver
201  {
202  public:
205  ODEBasicSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
206  : ODESolver(si, ode, intStep)
207  {
208  }
209 
210  protected:
212  void solve(StateType &state, const Control *control, double duration) const override
213  {
214  Solver solver;
215  ODESolver::ODEFunctor odefunc(ode_, control);
216  odeint::integrate_const(solver, odefunc, state, 0.0, duration, intStep_);
217  }
218  };
219 
226  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType>>
227  class ODEErrorSolver : public ODESolver
228  {
229  public:
232  ODEErrorSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
233  : ODESolver(si, ode, intStep)
234  {
235  }
236 
239  {
240  return error_;
241  }
242 
243  protected:
245  void solve(StateType &state, const Control *control, double duration) const override
246  {
247  ODESolver::ODEFunctor odefunc(ode_, control);
248 
249  if (error_.size() != state.size())
250  error_.assign(state.size(), 0.0);
251 
252  Solver solver;
253  solver.adjust_size(state);
254 
255  double time = 0.0;
256  while (time < duration + std::numeric_limits<float>::epsilon())
257  {
258  solver.do_step(odefunc, state, time, intStep_, error_);
259  time += intStep_;
260  }
261  }
262 
265  };
266 
273  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType>>
275  {
276  public:
279  ODEAdaptiveSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
280  : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
281  {
282  }
283 
285  double getMaximumError() const
286  {
287  return maxError_;
288  }
289 
291  void setMaximumError(double error)
292  {
293  maxError_ = error;
294  }
295 
297  double getMaximumEpsilonError() const
298  {
299  return maxEpsilonError_;
300  }
301 
303  void setMaximumEpsilonError(double error)
304  {
305  maxEpsilonError_ = error;
306  }
307 
308  protected:
313  void solve(StateType &state, const Control *control, double duration) const override
314  {
315  ODESolver::ODEFunctor odefunc(ode_, control);
316 
317 #if BOOST_VERSION < 105600
318  odeint::controlled_runge_kutta<Solver> solver(
319  odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
320 #else
321  typename boost::numeric::odeint::result_of::make_controlled<Solver>::type solver =
322  make_controlled(1.0e-6, 1.0e-6, Solver());
323 #endif
324  odeint::integrate_adaptive(solver, odefunc, state, 0.0, duration, intStep_);
325  }
326 
328  double maxError_;
329 
332  };
333  }
334 }
335 
336 #endif
Solver for ordinary differential equations of the type q&#39; = f(q, u), where q is the current state of ...
Definition: ODESolver.h:227
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.
Definition: ODESolver.h:232
ODE ode_
Definition of the ODE to find solutions for.
Definition: ODESolver.h:168
Definition of an abstract control.
Definition: Control.h:47
double getIntegrationStepSize() const
Return the size of a single numerical integration step.
Definition: ODESolver.h:106
virtual void solve(StateType &state, const Control *control, double duration) const =0
Solve the ODE given the initial state, and a control to apply for some duration.
STL namespace.
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.
Definition: ODESolver.h:279
std::function< void(const base::State *, const Control *, double, base::State *)> PostPropagationEvent
Callback function to perform an event at the end of numerical integration. This functionality is opti...
Definition: ODESolver.h:87
void setODE(const ODE &ode)
Set the ODE to solve.
Definition: ODESolver.h:100
ODESolver::StateType getError()
Retrieves the error values from the most recent integration.
Definition: ODESolver.h:238
A shared pointer wrapper for ompl::control::ODESolver.
std::function< void(const StateType &, const Control *, StateType &)> ODE
Callback function that defines the ODE. Accepts the current state, input control, and output state...
Definition: ODESolver.h:82
static StatePropagatorPtr getStatePropagator(ODESolverPtr solver, const PostPropagationEvent &postEvent=nullptr)
Retrieve a StatePropagator object that solves a system of ordinary differential equations defined by ...
Definition: ODESolver.h:128
Model the effect of controls on system states.
void setMaximumError(double error)
Set the total error allowed during numerical integration.
Definition: ODESolver.h:291
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: ODESolver.h:118
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void solve(StateType &state, const Control *control, double duration) const override
Solve the ODE using boost::numeric::odeint. Save the resulting error values into error_.
Definition: ODESolver.h:245
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
virtual ~ODESolver()=default
Destructor.
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:78
Adaptive step size solver for ordinary differential equations of the type q&#39; = 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.
Definition: ODESolver.h:274
const SpaceInformationPtr si_
The SpaceInformation that this ODESolver operates in.
Definition: ODESolver.h:165
Definition of an abstract state.
Definition: State.h:49
void setIntegrationStepSize(double intStep)
Set the size of a single numerical integration step.
Definition: ODESolver.h:112
A shared pointer wrapper for ompl::control::SpaceInformation.
ODESolver(SpaceInformationPtr si, ODE ode, double intStep)
Parameterized constructor. Takes a reference to SpaceInformation, an ODE to solve, and the integration step size.
Definition: ODESolver.h:91
double getMaximumError() const
Retrieve the total error allowed during numerical integration.
Definition: ODESolver.h:285
ODESolver::StateType error_
The error values calculated during numerical integration.
Definition: ODESolver.h:264
double getMaximumEpsilonError() const
Retrieve the error tolerance during one step of numerical integration (local truncation error) ...
Definition: ODESolver.h:297
Basic solver for ordinary differential equations of the type q&#39; = f(q, u), where q is the current sta...
Definition: ODESolver.h:200
void solve(StateType &state, const Control *control, double duration) const override
Solve the ODE using boost::numeric::odeint.
Definition: ODESolver.h:212
void setMaximumEpsilonError(double error)
Set the error tolerance during one step of numerical integration (local truncation error) ...
Definition: ODESolver.h:303
double maxError_
The maximum error allowed when performing numerical integration.
Definition: ODESolver.h:328
Abstract base class for an object that can solve ordinary differential equations (ODE) of the type q&#39;...
Definition: ODESolver.h:74
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.
Definition: ODESolver.h:205
void solve(StateType &state, const Control *control, double duration) const override
Solve the ordinary differential equation given the input state of the system, a control to apply to t...
Definition: ODESolver.h:313
double intStep_
The size of the numerical integration step. Should be small to minimize error.
Definition: ODESolver.h:171
double maxEpsilonError_
The maximum error allowed during one step of numerical integration.
Definition: ODESolver.h:331