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/numeric/odeint/stepper/runge_kutta4.hpp>
47 #include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
48 #include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
49 #include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
50 #include <boost/numeric/odeint/integrate/integrate_const.hpp>
51 #include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
52 namespace odeint = boost::numeric::odeint;
53 #include <functional>
54 #include <cassert>
55 #include <utility>
56 #include <vector>
57 
58 namespace ompl
59 {
60  namespace control
61  {
63  OMPL_CLASS_FORWARD(ODESolver);
65 
68 
73  class ODESolver
74  {
75  public:
77  using StateType = std::vector<double>;
78 
81  using ODE = std::function<void(const StateType &, const Control *, StateType &)>;
82 
86  std::function<void(const base::State *, const Control *, double, base::State *)>;
87 
90  ODESolver(SpaceInformationPtr si, ODE ode, double intStep)
91  : si_(std::move(si)), ode_(std::move(ode)), intStep_(intStep)
92  {
93  }
94 
96  virtual ~ODESolver() = default;
97 
99  void setODE(const ODE &ode)
100  {
101  ode_ = ode;
102  }
103 
105  double getIntegrationStepSize() const
106  {
107  return intStep_;
108  }
109 
111  void setIntegrationStepSize(double intStep)
112  {
113  intStep_ = intStep;
114  }
115 
118  {
119  return si_;
120  }
121 
128  const PostPropagationEvent &postEvent = nullptr)
129  {
130  class ODESolverStatePropagator : public StatePropagator
131  {
132  public:
133  ODESolverStatePropagator(const ODESolverPtr& solver, PostPropagationEvent pe)
134  : StatePropagator(solver->si_), solver_(solver), postEvent_(std::move(pe))
135  {
136  if (solver == nullptr)
137  OMPL_ERROR("ODESolverPtr does not reference a valid ODESolver object");
138  }
139 
140  void propagate(const base::State *state, const Control *control, double duration,
141  base::State *result) const override
142  {
143  ODESolver::StateType reals;
144  si_->getStateSpace()->copyToReals(reals, state);
145  solver_->solve(reals, control, duration);
146  si_->getStateSpace()->copyFromReals(result, reals);
147 
148  if (postEvent_)
149  postEvent_(state, control, duration, result);
150  }
151 
152  protected:
153  ODESolverPtr solver_;
155  };
156  return std::make_shared<ODESolverStatePropagator>(std::move(solver), postEvent);
157  }
158 
159  protected:
161  virtual void solve(StateType &state, const Control *control, double duration) const = 0;
162 
165 
168 
170  double intStep_;
171 
173  // Functor used by the boost::numeric::odeint stepper object
174  struct ODEFunctor
175  {
176  ODEFunctor(ODE o, const Control *ctrl) : ode(std::move(o)), control(ctrl)
177  {
178  }
179 
180  // boost::numeric::odeint will callback to this method during integration to evaluate the system
181  void operator()(const StateType &current, StateType &output, double /*time*/)
182  {
183  ode(current, control, output);
184  }
185 
186  ODE ode;
187  const Control *control;
188  };
190  };
191 
198  template <class Solver = odeint::runge_kutta4<ODESolver::StateType>>
199  class ODEBasicSolver : public ODESolver
200  {
201  public:
204  ODEBasicSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
205  : ODESolver(si, ode, intStep)
206  {
207  }
208 
209  protected:
211  void solve(StateType &state, const Control *control, double duration) const override
212  {
213  Solver solver;
214  ODESolver::ODEFunctor odefunc(ode_, control);
215  odeint::integrate_const(solver, odefunc, state, 0.0, duration, intStep_);
216  }
217  };
218 
225  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType>>
226  class ODEErrorSolver : public ODESolver
227  {
228  public:
231  ODEErrorSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
232  : ODESolver(si, ode, intStep)
233  {
234  }
235 
238  {
239  return error_;
240  }
241 
242  protected:
244  void solve(StateType &state, const Control *control, double duration) const override
245  {
246  ODESolver::ODEFunctor odefunc(ode_, control);
247 
248  if (error_.size() != state.size())
249  error_.assign(state.size(), 0.0);
250 
251  Solver solver;
252  solver.adjust_size(state);
253 
254  double time = 0.0;
255  while (time < duration + std::numeric_limits<float>::epsilon())
256  {
257  solver.do_step(odefunc, state, time, intStep_, error_);
258  time += intStep_;
259  }
260  }
261 
264  };
265 
272  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType>>
274  {
275  public:
278  ODEAdaptiveSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
279  : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
280  {
281  }
282 
284  double getMaximumError() const
285  {
286  return maxError_;
287  }
288 
290  void setMaximumError(double error)
291  {
292  maxError_ = error;
293  }
294 
296  double getMaximumEpsilonError() const
297  {
298  return maxEpsilonError_;
299  }
300 
302  void setMaximumEpsilonError(double error)
303  {
304  maxEpsilonError_ = error;
305  }
306 
307  protected:
312  void solve(StateType &state, const Control *control, double duration) const override
313  {
314  ODESolver::ODEFunctor odefunc(ode_, control);
315  auto solver = make_controlled(1.0e-6, 1.0e-6, Solver());
316  odeint::integrate_adaptive(solver, odefunc, state, 0.0, duration, intStep_);
317  }
318 
320  double maxError_;
321 
324  };
325  }
326 }
327 
328 #endif
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::control::StatePropagator.
Definition of an abstract state.
Definition: State.h:50
Definition of an abstract control.
Definition: Control.h:48
Adaptive step size solver for ordinary differential equations of the type q' = f(q,...
Definition: ODESolver.h:274
double maxError_
The maximum error allowed when performing numerical integration.
Definition: ODESolver.h:320
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:312
double getMaximumEpsilonError() const
Retrieve the error tolerance during one step of numerical integration (local truncation error)
Definition: ODESolver.h:296
double maxEpsilonError_
The maximum error allowed during one step of numerical integration.
Definition: ODESolver.h:323
void setMaximumEpsilonError(double error)
Set the error tolerance during one step of numerical integration (local truncation error)
Definition: ODESolver.h:302
void setMaximumError(double error)
Set the total error allowed during numerical integration.
Definition: ODESolver.h:290
double getMaximumError() const
Retrieve the total error allowed during numerical integration.
Definition: ODESolver.h:284
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:278
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:200
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:204
void solve(StateType &state, const Control *control, double duration) const override
Solve the ODE using boost::numeric::odeint.
Definition: ODESolver.h:211
Solver for ordinary differential equations of the type q' = 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,...
Definition: ODESolver.h:231
ODESolver::StateType getError()
Retrieves the error values from the most recent integration.
Definition: ODESolver.h:237
ODESolver::StateType error_
The error values calculated during numerical integration.
Definition: ODESolver.h:263
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:244
A shared pointer wrapper for ompl::control::ODESolver.
Abstract base class for an object that can solve ordinary differential equations (ODE) of the type q'...
Definition: ODESolver.h:74
double intStep_
The size of the numerical integration step. Should be small to minimize error.
Definition: ODESolver.h:170
ODE ode_
Definition of the ODE to find solutions for.
Definition: ODESolver.h:167
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.
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:81
virtual ~ODESolver()=default
Destructor.
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: ODESolver.h:117
const SpaceInformationPtr si_
The SpaceInformation that this ODESolver operates in.
Definition: ODESolver.h:164
void setODE(const ODE &ode)
Set the ODE to solve.
Definition: ODESolver.h:99
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:77
double getIntegrationStepSize() const
Return the size of a single numerical integration step.
Definition: ODESolver.h:105
void setIntegrationStepSize(double intStep)
Set the size of a single numerical integration step.
Definition: ODESolver.h:111
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:127
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:86
ODESolver(SpaceInformationPtr si, ODE ode, double intStep)
Parameterized constructor. Takes a reference to SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:90
Model the effect of controls on system states.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Main namespace. Contains everything in this library.
Definition: AppBase.h:22