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 #include <boost/numeric/odeint.hpp>
53 #include <boost/function.hpp>
54 #include <cassert>
55 #include <vector>
56 
57 namespace ompl
58 {
59 
60  namespace control
61  {
62 
64  OMPL_CLASS_FORWARD(ODESolver);
66 
69 
74  class ODESolver
75  {
76  public:
78  typedef std::vector<double> StateType;
79 
82  typedef boost::function<void(const StateType &, const Control*, StateType &)> ODE;
83 
86  typedef boost::function<void(const base::State *state, const Control* control, const double duration, base::State *result)> PostPropagationEvent;
87 
90  ODESolver (const SpaceInformationPtr& si, const ODE& ode, double intStep) : si_(si), ode_(ode), intStep_(intStep)
91  {
92  }
93 
95  virtual ~ODESolver (void)
96  {
97  }
98 
100  void setODE (const ODE &ode)
101  {
102  ode_ = ode;
103  }
104 
106  double getIntegrationStepSize (void) const
107  {
108  return intStep_;
109  }
110 
112  void setIntegrationStepSize (double intStep)
113  {
114  intStep_ = intStep;
115  }
116 
119  {
120  return si_;
121  }
122 
128  static StatePropagatorPtr getStatePropagator (ODESolverPtr solver,
129  const PostPropagationEvent &postEvent = NULL)
130  {
131  class ODESolverStatePropagator : public StatePropagator
132  {
133  public:
134  ODESolverStatePropagator (ODESolverPtr solver, const PostPropagationEvent &pe) : StatePropagator (solver->si_), solver_(solver), postEvent_(pe)
135  {
136  if (!solver.get())
137  OMPL_ERROR("ODESolverPtr does not reference a valid ODESolver object");
138  }
139 
140  virtual void propagate (const base::State *state, const Control* control, const double duration, base::State *result) const
141  {
142  ODESolver::StateType reals;
143  si_->getStateSpace()->copyToReals(reals, state);
144  solver_->solve (reals, control, duration);
145  si_->getStateSpace()->copyFromReals(result, reals);
146 
147  if (postEvent_)
148  postEvent_ (state, control, duration, result);
149  }
150 
151  protected:
152  ODESolverPtr solver_;
154  };
155  return StatePropagatorPtr(dynamic_cast<StatePropagator*>(new ODESolverStatePropagator(solver, postEvent)));
156  }
157 
158  protected:
159 
161  virtual void solve (StateType &state, const Control* control, const 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 (const ODE &o, const Control* ctrl) : ode(o), control(ctrl) {}
177 
178  // boost::numeric::odeint will callback to this method during integration to evaluate the system
179  void operator () (const StateType &current, StateType &output, double /*time*/)
180  {
181  ode (current, control, output);
182  }
183 
184  ODE ode;
185  const Control* control;
186  };
188  };
189 
196  template <class Solver = boost::numeric::odeint::runge_kutta4<ODESolver::StateType> >
197  class ODEBasicSolver : public ODESolver
198  {
199  public:
200 
203  ODEBasicSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
204  {
205  }
206 
207  protected:
208 
210  virtual void solve (StateType &state, const Control* control, const double duration) const
211  {
212  Solver solver;
213  ODESolver::ODEFunctor odefunc (ode_, control);
214  boost::numeric::odeint::integrate_const (solver, odefunc, state, 0.0, duration, intStep_);
215  }
216  };
217 
224  template <class Solver = boost::numeric::odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
225  class ODEErrorSolver : public ODESolver
226  {
227  public:
230  ODEErrorSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
231  {
232  }
233 
236  {
237  ODESolver::StateType error (error_.begin (), error_.end ());
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)
255  {
256  solver.do_step (odefunc, state, time, intStep_, error_);
257  time += intStep_;
258  }
259  }
260 
263  };
264 
271  template <class Solver = boost::numeric::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 (void) const
283  {
284  return maxError_;
285  }
286 
288  void setMaximumError (double error)
289  {
290  maxError_ = error;
291  }
292 
294  double getMaximumEpsilonError (void) 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  boost::numeric::odeint::controlled_runge_kutta< Solver > solver (boost::numeric::odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
316  boost::numeric::odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration, intStep_);
317  }
318 
320  double maxError_;
321 
324  };
325  }
326 }
327 
328 #endif
329 
330 #endif