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 
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 
165  const SpaceInformationPtr si_;
166 
168  ODE ode_;
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>>
274  class ODEAdaptiveSolver : public ODESolver
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 
331  double maxEpsilonError_;
332  };
333  }
334 }
335 
336 #endif
double getIntegrationStepSize() const
Return the size of a single numerical integration step.
Definition: ODESolver.h:170
double maxError_
The maximum error allowed when performing numerical integration.
Definition: ODESolver.h:392
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:151
Definition of an abstract control.
Definition: Control.h:111
A shared pointer wrapper for ompl::base::SpaceInformation.
ODESolver::StateType error_
The error values calculated during numerical integration.
Definition: ODESolver.h:328
void setODE(const ODE &ode)
Set the ODE to solve.
Definition: ODESolver.h:164
Definition of an abstract state.
Definition: State.h:113
ODE ode_
Definition of the ODE to find solutions for.
Definition: ODESolver.h:232
double intStep_
The size of the numerical integration step. Should be small to minimize error.
Definition: ODESolver.h:235
double getMaximumError() const
Retrieve the total error allowed during numerical integration.
Definition: ODESolver.h:349
ODEErrorSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:296
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:142
ODEBasicSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:269
Adaptive step size solver for ordinary differential equations of the type q' = f(q,...
Definition: ODESolver.h:338
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:146
ODESolver(SpaceInformationPtr si, ODE ode, double intStep)
Parameterized constructor. Takes a reference to SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:155
void solve(StateType &state, const Control *control, double duration) const override
Solve the ODE using boost::numeric::odeint.
Definition: ODESolver.h:276
const SpaceInformationPtr si_
The SpaceInformation that this ODESolver operates in.
Definition: ODESolver.h:229
ODESolver::StateType getError()
Retrieves the error values from the most recent integration.
Definition: ODESolver.h:302
double maxEpsilonError_
The maximum error allowed during one step of numerical integration.
Definition: ODESolver.h:395
Solver for ordinary differential equations of the type q' = f(q, u), where q is the current state of ...
Definition: ODESolver.h:291
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:309
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:192
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: ODESolver.h:182
double getMaximumEpsilonError() const
Retrieve the error tolerance during one step of numerical integration (local truncation error)
Definition: ODESolver.h:361
virtual ~ODESolver()=default
Destructor.
ODEAdaptiveSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:343
A shared pointer wrapper for ompl::control::ODESolver.
void setMaximumError(double error)
Set the total error allowed during numerical integration.
Definition: ODESolver.h:355
void setIntegrationStepSize(double intStep)
Set the size of a single numerical integration step.
Definition: ODESolver.h:176
A shared pointer wrapper for ompl::control::StatePropagator.
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:377
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:264
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.
Abstract base class for an object that can solve ordinary differential equations (ODE) of the type q'...
Definition: ODESolver.h:138
void setMaximumEpsilonError(double error)
Set the error tolerance during one step of numerical integration (local truncation error)
Definition: ODESolver.h:367
Main namespace. Contains everything in this library.