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 #if BOOST_VERSION >= 105300
48 #include <boost/numeric/odeint.hpp>
49 namespace odeint = boost::numeric::odeint;
50 #else
51 #include <omplext_odeint/boost/numeric/odeint.hpp>
52 namespace odeint = boost::numeric::omplext_odeint;
53 #endif
54 #include <boost/function.hpp>
55 #include <cassert>
56 #include <vector>
57 
58 namespace ompl
59 {
60 
61  namespace control
62  {
63 
65  OMPL_CLASS_FORWARD(ODESolver);
67 
70 
75  class ODESolver
76  {
77  public:
79  typedef std::vector<double> StateType;
80 
83  typedef boost::function<void(const StateType &, const Control*, StateType &)> ODE;
84 
87  typedef boost::function<void(const base::State *state, const Control *control, const double duration, base::State *result)> PostPropagationEvent;
88 
91  ODESolver (const SpaceInformationPtr& si, const ODE& ode, double intStep) : si_(si), ode_(ode), intStep_(intStep)
92  {
93  }
94 
96  virtual ~ODESolver ()
97  {
98  }
99 
101  void setODE (const ODE &ode)
102  {
103  ode_ = ode;
104  }
105 
107  double getIntegrationStepSize () const
108  {
109  return intStep_;
110  }
111 
113  void setIntegrationStepSize (double intStep)
114  {
115  intStep_ = intStep;
116  }
117 
120  {
121  return si_;
122  }
123 
129  static StatePropagatorPtr getStatePropagator (ODESolverPtr solver,
130  const PostPropagationEvent &postEvent = NULL)
131  {
132  class ODESolverStatePropagator : public StatePropagator
133  {
134  public:
135  ODESolverStatePropagator (ODESolverPtr solver, const PostPropagationEvent &pe) : StatePropagator (solver->si_), solver_(solver), postEvent_(pe)
136  {
137  if (!solver.get())
138  OMPL_ERROR("ODESolverPtr does not reference a valid ODESolver object");
139  }
140 
141  virtual void propagate (const base::State *state, const Control *control, const double duration, base::State *result) const
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 StatePropagatorPtr(dynamic_cast<StatePropagator*>(new ODESolverStatePropagator(solver, postEvent)));
157  }
158 
159  protected:
160 
162  virtual void solve (StateType &state, const Control *control, const double duration) const = 0;
163 
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 (const ODE &o, const Control *ctrl) : ode(o), control(ctrl) {}
178 
179  // boost::numeric::odeint will callback to this method during integration to evaluate the system
180  void operator () (const StateType &current, StateType &output, double /*time*/)
181  {
182  ode (current, control, output);
183  }
184 
185  ODE ode;
186  const Control *control;
187  };
189  };
190 
197  template <class Solver = odeint::runge_kutta4<ODESolver::StateType> >
198  class ODEBasicSolver : public ODESolver
199  {
200  public:
201 
204  ODEBasicSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
205  {
206  }
207 
208  protected:
209 
211  virtual void solve (StateType &state, const Control *control, const double duration) const
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) : ODESolver(si, ode, intStep)
232  {
233  }
234 
237  {
238  return error_;
239  }
240 
241  protected:
243  virtual void solve (StateType &state, const Control *control, const double duration) const
244  {
245  ODESolver::ODEFunctor odefunc (ode_, control);
246 
247  if (error_.size () != state.size ())
248  error_.assign (state.size (), 0.0);
249 
250  Solver solver;
251  solver.adjust_size (state);
252 
253  double time = 0.0;
254  while (time < duration + std::numeric_limits<float>::epsilon())
255  {
256  solver.do_step (odefunc, state, time, intStep_, error_);
257  time += intStep_;
258  }
259  }
260 
263  };
264 
271  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
273  {
274  public:
277  ODEAdaptiveSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
278  {
279  }
280 
282  double getMaximumError () const
283  {
284  return maxError_;
285  }
286 
288  void setMaximumError (double error)
289  {
290  maxError_ = error;
291  }
292 
294  double getMaximumEpsilonError () const
295  {
296  return maxEpsilonError_;
297  }
298 
300  void setMaximumEpsilonError (double error)
301  {
302  maxEpsilonError_ = error;
303  }
304 
305  protected:
306 
311  virtual void solve (StateType &state, const Control *control, const double duration) const
312  {
313  ODESolver::ODEFunctor odefunc (ode_, control);
314 
315 #if BOOST_VERSION < 105600
316  odeint::controlled_runge_kutta< Solver > solver (odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
317 #else
318  typename boost::numeric::odeint::result_of::make_controlled< Solver >::type solver = make_controlled( 1.0e-6 , 1.0e-6 , Solver() );
319 #endif
320  odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration, intStep_);
321  }
322 
324  double maxError_;
325 
328  };
329  }
330 }
331 
332 #endif
Solver for ordinary differential equations of the type q&#39; = f(q, u), where q is the current state of ...
Definition: ODESolver.h:226
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:231
ODE ode_
Definition of the ODE to find solutions for.
Definition: ODESolver.h:168
Definition of an abstract control.
Definition: Control.h:48
double getIntegrationStepSize() const
Return the size of a single numerical integration step.
Definition: ODESolver.h:107
static StatePropagatorPtr getStatePropagator(ODESolverPtr solver, const PostPropagationEvent &postEvent=NULL)
Retrieve a StatePropagator object that solves a system of ordinary differential equations defined by ...
Definition: ODESolver.h:129
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:277
boost::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:83
void setODE(const ODE &ode)
Set the ODE to solve.
Definition: ODESolver.h:101
ODESolver::StateType getError()
Retrieves the error values from the most recent integration.
Definition: ODESolver.h:236
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.
Definition: ODESolver.h:288
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: ODESolver.h:119
Main namespace. Contains everything in this library.
Definition: Cost.h:42
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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.
Definition: ODESolver.h:79
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:272
const SpaceInformationPtr si_
The SpaceInformation that this ODESolver operates in.
Definition: ODESolver.h:165
virtual void solve(StateType &state, const Control *control, const double duration) const
Solve the ODE using boost::numeric::odeint.
Definition: ODESolver.h:211
Definition of an abstract state.
Definition: State.h:50
void setIntegrationStepSize(double intStep)
Set the size of a single numerical integration step.
Definition: ODESolver.h:113
A boost shared pointer wrapper for ompl::control::SpaceInformation.
double getMaximumError() const
Retrieve the total error allowed during numerical integration.
Definition: ODESolver.h:282
ODESolver::StateType error_
The error values calculated during numerical integration.
Definition: ODESolver.h:262
double getMaximumEpsilonError() const
Retrieve the error tolerance during one step of numerical integration (local truncation error) ...
Definition: ODESolver.h:294
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...
Definition: ODESolver.h:87
virtual ~ODESolver()
Destructor.
Definition: ODESolver.h:96
Basic solver for ordinary differential equations of the type q&#39; = f(q, u), where q is the current sta...
Definition: ODESolver.h:198
void setMaximumEpsilonError(double error)
Set the error tolerance during one step of numerical integration (local truncation error) ...
Definition: ODESolver.h:300
double maxError_
The maximum error allowed when performing numerical integration.
Definition: ODESolver.h:324
Abstract base class for an object that can solve ordinary differential equations (ODE) of the type q&#39;...
Definition: ODESolver.h:75
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...
Definition: ODESolver.h:311
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_.
Definition: ODESolver.h:243
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:204
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.
Definition: ODESolver.h:91
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:327