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.hpp>
48 namespace odeint = boost::numeric::odeint;
49 #include <functional>
50 #include <cassert>
51 #include <utility>
52 #include <vector>
53 
54 namespace ompl
55 {
56  namespace control
57  {
59  OMPL_CLASS_FORWARD(ODESolver);
61 
64 
69  class ODESolver
70  {
71  public:
73  typedef std::vector<double> StateType;
74 
77  typedef std::function<void(const StateType &, const Control *, StateType &)> ODE;
78 
81  typedef std::function<void(const base::State *, const Control *, double, base::State *)>
83 
86  ODESolver(SpaceInformationPtr si, ODE ode, double intStep)
87  : si_(std::move(si)), ode_(std::move(ode)), intStep_(intStep)
88  {
89  }
90 
92  virtual ~ODESolver() = default;
93 
95  void setODE(const ODE &ode)
96  {
97  ode_ = ode;
98  }
99 
101  double getIntegrationStepSize() const
102  {
103  return intStep_;
104  }
105 
107  void setIntegrationStepSize(double intStep)
108  {
109  intStep_ = intStep;
110  }
111 
114  {
115  return si_;
116  }
117 
123  static StatePropagatorPtr getStatePropagator(ODESolverPtr solver,
124  const PostPropagationEvent &postEvent = nullptr)
125  {
126  class ODESolverStatePropagator : public StatePropagator
127  {
128  public:
129  ODESolverStatePropagator(const ODESolverPtr& solver, PostPropagationEvent pe)
130  : StatePropagator(solver->si_), solver_(solver), postEvent_(std::move(pe))
131  {
132  if (solver == nullptr)
133  OMPL_ERROR("ODESolverPtr does not reference a valid ODESolver object");
134  }
135 
136  void propagate(const base::State *state, const Control *control, double duration,
137  base::State *result) const override
138  {
139  ODESolver::StateType reals;
140  si_->getStateSpace()->copyToReals(reals, state);
141  solver_->solve(reals, control, duration);
142  si_->getStateSpace()->copyFromReals(result, reals);
143 
144  if (postEvent_)
145  postEvent_(state, control, duration, result);
146  }
147 
148  protected:
149  ODESolverPtr solver_;
151  };
152  return std::make_shared<ODESolverStatePropagator>(std::move(solver), postEvent);
153  }
154 
155  protected:
157  virtual void solve(StateType &state, const Control *control, double duration) const = 0;
158 
161 
163  ODE ode_;
164 
166  double intStep_;
167 
169  // Functor used by the boost::numeric::odeint stepper object
170  struct ODEFunctor
171  {
172  ODEFunctor(ODE o, const Control *ctrl) : ode(std::move(o)), control(ctrl)
173  {
174  }
175 
176  // boost::numeric::odeint will callback to this method during integration to evaluate the system
177  void operator()(const StateType &current, StateType &output, double /*time*/)
178  {
179  ode(current, control, output);
180  }
181 
182  ODE ode;
183  const Control *control;
184  };
186  };
187 
194  template <class Solver = odeint::runge_kutta4<ODESolver::StateType>>
195  class ODEBasicSolver : public ODESolver
196  {
197  public:
200  ODEBasicSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
201  : ODESolver(si, ode, intStep)
202  {
203  }
204 
205  protected:
207  void solve(StateType &state, const Control *control, double duration) const override
208  {
209  Solver solver;
210  ODESolver::ODEFunctor odefunc(ode_, control);
211  odeint::integrate_const(solver, odefunc, state, 0.0, duration, intStep_);
212  }
213  };
214 
221  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType>>
222  class ODEErrorSolver : public ODESolver
223  {
224  public:
227  ODEErrorSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
228  : ODESolver(si, ode, intStep)
229  {
230  }
231 
234  {
235  return error_;
236  }
237 
238  protected:
240  void solve(StateType &state, const Control *control, double duration) const override
241  {
242  ODESolver::ODEFunctor odefunc(ode_, control);
243 
244  if (error_.size() != state.size())
245  error_.assign(state.size(), 0.0);
246 
247  Solver solver;
248  solver.adjust_size(state);
249 
250  double time = 0.0;
251  while (time < duration + std::numeric_limits<float>::epsilon())
252  {
253  solver.do_step(odefunc, state, time, intStep_, error_);
254  time += intStep_;
255  }
256  }
257 
260  };
261 
268  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType>>
270  {
271  public:
274  ODEAdaptiveSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
275  : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
276  {
277  }
278 
280  double getMaximumError() const
281  {
282  return maxError_;
283  }
284 
286  void setMaximumError(double error)
287  {
288  maxError_ = error;
289  }
290 
292  double getMaximumEpsilonError() const
293  {
294  return maxEpsilonError_;
295  }
296 
298  void setMaximumEpsilonError(double error)
299  {
300  maxEpsilonError_ = error;
301  }
302 
303  protected:
308  void solve(StateType &state, const Control *control, double duration) const override
309  {
310  ODESolver::ODEFunctor odefunc(ode_, control);
311 
312 #if BOOST_VERSION < 105600
313  odeint::controlled_runge_kutta<Solver> solver(
314  odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
315 #else
316  typename boost::numeric::odeint::result_of::make_controlled<Solver>::type solver =
317  make_controlled(1.0e-6, 1.0e-6, Solver());
318 #endif
319  odeint::integrate_adaptive(solver, odefunc, state, 0.0, duration, intStep_);
320  }
321 
323  double maxError_;
324 
327  };
328  }
329 }
330 
331 #endif
Solver for ordinary differential equations of the type q&#39; = f(q, u), where q is the current state of ...
Definition: ODESolver.h:222
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:227
ODE ode_
Definition of the ODE to find solutions for.
Definition: ODESolver.h:163
Definition of an abstract control.
Definition: Control.h:47
double getIntegrationStepSize() const
Return the size of a single numerical integration step.
Definition: ODESolver.h:101
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:274
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:82
void setODE(const ODE &ode)
Set the ODE to solve.
Definition: ODESolver.h:95
ODESolver::StateType getError()
Retrieves the error values from the most recent integration.
Definition: ODESolver.h:233
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:77
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:123
Model the effect of controls on system states.
void setMaximumError(double error)
Set the total error allowed during numerical integration.
Definition: ODESolver.h:286
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: ODESolver.h:113
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:240
#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:73
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:269
const SpaceInformationPtr si_
The SpaceInformation that this ODESolver operates in.
Definition: ODESolver.h:160
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:107
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:86
double getMaximumError() const
Retrieve the total error allowed during numerical integration.
Definition: ODESolver.h:280
ODESolver::StateType error_
The error values calculated during numerical integration.
Definition: ODESolver.h:259
double getMaximumEpsilonError() const
Retrieve the error tolerance during one step of numerical integration (local truncation error) ...
Definition: ODESolver.h:292
Basic solver for ordinary differential equations of the type q&#39; = f(q, u), where q is the current sta...
Definition: ODESolver.h:195
void solve(StateType &state, const Control *control, double duration) const override
Solve the ODE using boost::numeric::odeint.
Definition: ODESolver.h:207
void setMaximumEpsilonError(double error)
Set the error tolerance during one step of numerical integration (local truncation error) ...
Definition: ODESolver.h:298
double maxError_
The maximum error allowed when performing numerical integration.
Definition: ODESolver.h:323
Abstract base class for an object that can solve ordinary differential equations (ODE) of the type q&#39;...
Definition: ODESolver.h:69
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:200
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:308
double intStep_
The size of the numerical integration step. Should be small to minimize error.
Definition: ODESolver.h:166
double maxEpsilonError_
The maximum error allowed during one step of numerical integration.
Definition: ODESolver.h:326