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 
51 #include <omplext_odeint/boost/numeric/odeint.hpp>
52 #include <boost/function.hpp>
53 #include <cassert>
54 #include <vector>
55 
56 namespace ompl
57 {
58 
59  namespace control
60  {
61 
66  class ODESolver
67  {
68  public:
70  typedef std::vector<double> StateType;
71 
74  typedef boost::function<void(const StateType &, const Control*, StateType &)> ODE;
75 
78  typedef boost::function<void(const Control*, base::State*)> PostPropagationEvent;
79 
82  ODESolver (const SpaceInformationPtr &si, const ODE &ode, double intStep) : si_(si), ode_(ode), intStep_(intStep)
83  {
84  }
85 
87  virtual ~ODESolver (void)
88  {
89  }
90 
92  void setODE (const ODE &ode)
93  {
94  ode_ = ode;
95  }
96 
98  double getIntegrationStepSize (void) const
99  {
100  return intStep_;
101  }
102 
104  void setIntegrationStepSize (double intStep)
105  {
106  intStep_ = intStep;
107  }
108 
114  StatePropagatorPtr getStatePropagator (const PostPropagationEvent &postEvent = NULL) const
115  {
116  class ODESolverStatePropagator : public StatePropagator
117  {
118  public:
119  ODESolverStatePropagator (const SpaceInformationPtr& si, const ODESolver *solver, const PostPropagationEvent &pe) : StatePropagator (si), solver_(solver), postEvent_(pe)
120  {
121  }
122 
123  virtual void propagate (const base::State *state, const Control* control, const double duration, base::State *result) const
124  {
125  ODESolver::StateType reals;
126  si_->getStateSpace()->copyToReals(reals, state);
127  solver_->solve (reals, control, duration);
128  si_->getStateSpace()->copyFromReals(result, reals);
129 
130  if (postEvent_)
131  postEvent_ (control, result);
132  }
133 
134  protected:
135  const ODESolver *solver_;
137  };
138 
139  return StatePropagatorPtr(dynamic_cast<StatePropagator*>(new ODESolverStatePropagator(si_, this, postEvent)));
140  }
141 
142  protected:
143 
145  virtual void solve (StateType &state, const Control* control, const double duration) const = 0;
146 
149 
152 
154  double intStep_;
155 
157  // Functor used by the boost::numeric::odeint stepper object
158  struct ODEFunctor
159  {
160  ODEFunctor (const ODE &o, const Control* ctrl) : ode(o), control(ctrl) {}
161 
162  // boost::numeric::odeint will callback to this method during integration to evaluate the system
163  void operator () (const StateType &current, StateType &output, double /*time*/)
164  {
165  ode (current, control, output);
166  }
167 
168  ODE ode;
169  const Control* control;
170  };
172  };
173 
180  template <class Solver = boost::numeric::omplext_odeint::runge_kutta4<ODESolver::StateType> >
181  class ODEBasicSolver : public ODESolver
182  {
183  public:
184 
187  ODEBasicSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
188  {
189  }
190 
191  protected:
192 
194  virtual void solve (StateType &state, const Control* control, const double duration) const
195  {
196  Solver solver;
197  ODESolver::ODEFunctor odefunc (ode_, control);
198  boost::numeric::omplext_odeint::integrate_const (solver, odefunc, state, 0.0, duration, intStep_);
199  }
200  };
201 
208  template <class Solver = boost::numeric::omplext_odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
209  class ODEErrorSolver : public ODESolver
210  {
211  public:
214  ODEErrorSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
215  {
216  }
217 
220  {
221  ODESolver::StateType error (error_.begin (), error_.end ());
222  return error;
223  }
224 
225  protected:
227  virtual void solve (StateType &state, const Control* control, const double duration) const
228  {
229  ODESolver::ODEFunctor odefunc (ode_, control);
230 
231  if (error_.size () != state.size ())
232  error_.assign (state.size (), 0.0);
233 
234  Solver solver;
235  solver.adjust_size (state);
236 
237  double time = 0.0;
238  while (time < duration)
239  {
240  solver.do_step (odefunc, state, time, intStep_, error_);
241  time += intStep_;
242  }
243  }
244 
247  };
248 
255  template <class Solver = boost::numeric::omplext_odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
257  {
258  public:
261  ODEAdaptiveSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
262  {
263  }
264 
266  double getMaximumError (void) const
267  {
268  return maxError_;
269  }
270 
272  void setMaximumError (double error)
273  {
274  maxError_ = error;
275  }
276 
278  double getMaximumEpsilonError (void) const
279  {
280  return maxEpsilonError_;
281  }
282 
284  void setMaximumEpsilonError (double error)
285  {
286  maxEpsilonError_ = error;
287  }
288 
289  protected:
290 
295  virtual void solve (StateType &state, const Control* control, const double duration) const
296  {
297  ODESolver::ODEFunctor odefunc (ode_, control);
298 
299  boost::numeric::omplext_odeint::controlled_runge_kutta< Solver > solver (boost::numeric::omplext_odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
300  boost::numeric::omplext_odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration, intStep_);
301  }
302 
304  double maxError_;
305 
308  };
309  }
310 }
311 
312 #endif
313 
314 #endif