All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 // Boost.OdeInt needs Boost version >= 1.44
41 #include <boost/version.hpp>
42 #if BOOST_VERSION < 104400
43 #warning Boost version >=1.44 is needed for ODESolver classes
44 #else
45 
46 #include "ompl/control/Control.h"
47 #include "ompl/control/SpaceInformation.h"
48 #include "ompl/control/StatePropagator.h"
49 #include "ompl/util/Console.h"
50 #include "ompl/util/ClassForward.h"
51 
52 #if BOOST_VERSION >= 105300
53 #include <boost/numeric/odeint.hpp>
54 namespace odeint = boost::numeric::odeint;
55 #else
56 #include <omplext_odeint/boost/numeric/odeint.hpp>
57 namespace odeint = boost::numeric::omplext_odeint;
58 #endif
59 #include <boost/function.hpp>
60 #include <cassert>
61 #include <vector>
62 
63 namespace ompl
64 {
65 
66  namespace control
67  {
68 
70  OMPL_CLASS_FORWARD(ODESolver);
72 
75 
80  class ODESolver
81  {
82  public:
84  typedef std::vector<double> StateType;
85 
88  typedef boost::function<void(const StateType &, const Control*, StateType &)> ODE;
89 
92  typedef boost::function<void(const base::State *state, const Control* control, const double duration, base::State *result)> PostPropagationEvent;
93 
96  ODESolver (const SpaceInformationPtr& si, const ODE& ode, double intStep) : si_(si), ode_(ode), intStep_(intStep)
97  {
98  }
99 
101  virtual ~ODESolver (void)
102  {
103  }
104 
106  void setODE (const ODE &ode)
107  {
108  ode_ = ode;
109  }
110 
112  double getIntegrationStepSize (void) const
113  {
114  return intStep_;
115  }
116 
118  void setIntegrationStepSize (double intStep)
119  {
120  intStep_ = intStep;
121  }
122 
125  {
126  return si_;
127  }
128 
134  static StatePropagatorPtr getStatePropagator (ODESolverPtr solver,
135  const PostPropagationEvent &postEvent = NULL)
136  {
137  class ODESolverStatePropagator : public StatePropagator
138  {
139  public:
140  ODESolverStatePropagator (ODESolverPtr solver, const PostPropagationEvent &pe) : StatePropagator (solver->si_), solver_(solver), postEvent_(pe)
141  {
142  if (!solver.get())
143  OMPL_ERROR("ODESolverPtr does not reference a valid ODESolver object");
144  }
145 
146  virtual void propagate (const base::State *state, const Control* control, const double duration, base::State *result) const
147  {
148  ODESolver::StateType reals;
149  si_->getStateSpace()->copyToReals(reals, state);
150  solver_->solve (reals, control, duration);
151  si_->getStateSpace()->copyFromReals(result, reals);
152 
153  if (postEvent_)
154  postEvent_ (state, control, duration, result);
155  }
156 
157  protected:
158  ODESolverPtr solver_;
160  };
161  return StatePropagatorPtr(dynamic_cast<StatePropagator*>(new ODESolverStatePropagator(solver, postEvent)));
162  }
163 
164  protected:
165 
167  virtual void solve (StateType &state, const Control* control, const double duration) const = 0;
168 
171 
174 
176  double intStep_;
177 
179  // Functor used by the boost::numeric::odeint stepper object
180  struct ODEFunctor
181  {
182  ODEFunctor (const ODE &o, const Control* ctrl) : ode(o), control(ctrl) {}
183 
184  // boost::numeric::odeint will callback to this method during integration to evaluate the system
185  void operator () (const StateType &current, StateType &output, double /*time*/)
186  {
187  ode (current, control, output);
188  }
189 
190  ODE ode;
191  const Control* control;
192  };
194  };
195 
202  template <class Solver = odeint::runge_kutta4<ODESolver::StateType> >
203  class ODEBasicSolver : public ODESolver
204  {
205  public:
206 
209  ODEBasicSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
210  {
211  }
212 
213  protected:
214 
216  virtual void solve (StateType &state, const Control* control, const double duration) const
217  {
218  Solver solver;
219  ODESolver::ODEFunctor odefunc (ode_, control);
220  odeint::integrate_const (solver, odefunc, state, 0.0, duration, intStep_);
221  }
222  };
223 
230  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
231  class ODEErrorSolver : public ODESolver
232  {
233  public:
236  ODEErrorSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
237  {
238  }
239 
242  {
243  ODESolver::StateType error (error_.begin (), error_.end ());
244  return error;
245  }
246 
247  protected:
249  virtual void solve (StateType &state, const Control* control, const double duration) const
250  {
251  ODESolver::ODEFunctor odefunc (ode_, control);
252 
253  if (error_.size () != state.size ())
254  error_.assign (state.size (), 0.0);
255 
256  Solver solver;
257  solver.adjust_size (state);
258 
259  double time = 0.0;
260  while (time < duration)
261  {
262  solver.do_step (odefunc, state, time, intStep_, error_);
263  time += intStep_;
264  }
265  }
266 
269  };
270 
277  template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
279  {
280  public:
283  ODEAdaptiveSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
284  {
285  }
286 
288  double getMaximumError (void) const
289  {
290  return maxError_;
291  }
292 
294  void setMaximumError (double error)
295  {
296  maxError_ = error;
297  }
298 
300  double getMaximumEpsilonError (void) const
301  {
302  return maxEpsilonError_;
303  }
304 
306  void setMaximumEpsilonError (double error)
307  {
308  maxEpsilonError_ = error;
309  }
310 
311  protected:
312 
317  virtual void solve (StateType &state, const Control* control, const double duration) const
318  {
319  ODESolver::ODEFunctor odefunc (ode_, control);
320 
321  odeint::controlled_runge_kutta< Solver > solver (odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
322  odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration, intStep_);
323  }
324 
326  double maxError_;
327 
330  };
331  }
332 }
333 
334 #endif
335 
336 #endif
Solver for ordinary differential equations of the type q' = f(q, u), where q is the current state of ...
Definition: ODESolver.h:231
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:236
ODE ode_
Definition of the ODE to find solutions for.
Definition: ODESolver.h:173
Definition of an abstract control.
Definition: Control.h:48
double getMaximumEpsilonError(void) const
Retrieve the error tolerance during one step of numerical integration (local truncation error) ...
Definition: ODESolver.h:300
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:134
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:283
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: ODESolver.h:124
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:88
void setODE(const ODE &ode)
Set the ODE to solve.
Definition: ODESolver.h:106
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:294
#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
Solve the ODE using boost::numeric::odeint. Save the resulting error values into error_.
Definition: ODESolver.h:249
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:84
virtual ~ODESolver(void)
Destructor.
Definition: ODESolver.h:101
Adaptive step size solver for ordinary differential equations of the type q' = 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:278
const SpaceInformationPtr si_
The SpaceInformation that this ODESolver operates in.
Definition: ODESolver.h:170
virtual void solve(StateType &state, const Control *control, const double duration) const
Solve the ODE using boost::numeric::odeint.
Definition: ODESolver.h:216
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:118
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:92
A boost shared pointer wrapper for ompl::control::SpaceInformation.
ODESolver::StateType error_
The error values calculated during numerical integration.
Definition: ODESolver.h:268
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:203
void setMaximumEpsilonError(double error)
Set the error tolerance during one step of numerical integration (local truncation error) ...
Definition: ODESolver.h:306
double maxError_
The maximum error allowed when performing numerical integration.
Definition: ODESolver.h:326
Abstract base class for an object that can solve ordinary differential equations (ODE) of the type q'...
Definition: ODESolver.h:80
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:209
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:96
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:317
ODESolver::StateType getError(void)
Retrieves the error values from the most recent integration.
Definition: ODESolver.h:241
double intStep_
The size of the numerical integration step. Should be small to minimize error.
Definition: ODESolver.h:176
double maxEpsilonError_
The maximum error allowed during one step of numerical integration.
Definition: ODESolver.h:329
double getMaximumError(void) const
Retrieve the total error allowed during numerical integration.
Definition: ODESolver.h:288
double getIntegrationStepSize(void) const
Return the size of a single numerical integration step.
Definition: ODESolver.h:112