All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
ODESolver.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ryan Luna */
00036 
00037 #ifndef OMPL_CONTROL_ODESOLVER_
00038 #define OMPL_CONTROL_ODESOLVER_
00039 
00040 // Boost.OdeInt needs Boost version >= 1.44
00041 #include <boost/version.hpp>
00042 #if BOOST_VERSION < 104400
00043 #warning Boost version >=1.44 is needed for ODESolver classes
00044 #else
00045 
00046 #include "ompl/control/Control.h"
00047 #include "ompl/control/SpaceInformation.h"
00048 #include "ompl/control/StatePropagator.h"
00049 #include "ompl/util/Console.h"
00050 
00051 #include <omplext_odeint/boost/numeric/odeint.hpp>
00052 #include <boost/function.hpp>
00053 #include <cassert>
00054 #include <vector>
00055 
00056 namespace ompl
00057 {
00058 
00059     namespace control
00060     {
00061 
00066         class ODESolver
00067         {
00068         public:
00070             typedef std::vector<double> StateType;
00071 
00074             typedef boost::function<void(const StateType &, const Control*, StateType &)> ODE;
00075 
00078             typedef boost::function<void(const Control*, base::State*)> PostPropagationEvent;
00079 
00082             ODESolver (const SpaceInformationPtr &si, const ODE &ode, double intStep) : si_(si), ode_(ode), intStep_(intStep), msg_("ODESolver")
00083             {
00084             }
00085 
00087             virtual ~ODESolver (void)
00088             {
00089             }
00090 
00092             void setODE (const ODE &ode)
00093             {
00094                 ode_ = ode;
00095             }
00096 
00098             double getIntegrationStepSize (void) const
00099             {
00100                 return intStep_;
00101             }
00102 
00104             void setIntegrationStepSize (double intStep)
00105             {
00106                 intStep_ = intStep;
00107             }
00108 
00114             StatePropagatorPtr getStatePropagator (const PostPropagationEvent &postEvent = NULL) const
00115             {
00116                 class ODESolverStatePropagator : public StatePropagator
00117                 {
00118                     public:
00119                         ODESolverStatePropagator (const SpaceInformationPtr& si, const ODESolver *solver, const PostPropagationEvent &pe) : StatePropagator (si), solver_(solver), postEvent_(pe)
00120                         {
00121                         }
00122 
00123                         virtual void propagate (const base::State *state, const Control* control, const double duration, base::State *result) const
00124                         {
00125                             ODESolver::StateType reals;
00126                             si_->getStateSpace()->copyToReals(reals, state);
00127                             solver_->solve (reals, control, duration);
00128                             si_->getStateSpace()->copyFromReals(result, reals);
00129 
00130                             if (postEvent_)
00131                                 postEvent_ (control, result);
00132                         }
00133 
00134                     protected:
00135                         const ODESolver *solver_;
00136                         ODESolver::PostPropagationEvent postEvent_;
00137                 };
00138 
00139                 return StatePropagatorPtr(dynamic_cast<StatePropagator*>(new ODESolverStatePropagator(si_, this, postEvent)));
00140             }
00141 
00142         protected:
00143 
00145             virtual void solve (StateType &state, const Control* control, const double duration) const = 0;
00146 
00148             const SpaceInformationPtr     si_;
00149 
00151             ODE                           ode_;
00152 
00154             double                        intStep_;
00155 
00157             msg::Interface                msg_;
00158 
00160             // Functor used by the boost::numeric::odeint stepper object
00161             struct ODEFunctor
00162             {
00163                 ODEFunctor (const ODE &o, const Control* ctrl) : ode(o), control(ctrl) {}
00164 
00165                 // boost::numeric::odeint will callback to this method during integration to evaluate the system
00166                 void operator () (const StateType &current, StateType &output, double /*time*/)
00167                 {
00168                     ode (current, control, output);
00169                 }
00170 
00171                 ODE ode;
00172                 const Control* control;
00173             };
00175         };
00176 
00183         template <class Solver = boost::numeric::omplext_odeint::runge_kutta4<ODESolver::StateType> >
00184         class ODEBasicSolver : public ODESolver
00185         {
00186         public:
00187 
00190             ODEBasicSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
00191             {
00192             }
00193 
00194         protected:
00195 
00197             virtual void solve (StateType &state, const Control* control, const double duration) const
00198             {
00199                 Solver solver;
00200                 ODESolver::ODEFunctor odefunc (ode_, control);
00201                 boost::numeric::omplext_odeint::integrate_const (solver, odefunc, state, 0.0, duration, intStep_);
00202             }
00203         };
00204 
00211         template <class Solver = boost::numeric::omplext_odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
00212         class ODEErrorSolver : public ODESolver
00213         {
00214         public:
00217             ODEErrorSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
00218             {
00219             }
00220 
00222             ODESolver::StateType getError (void)
00223             {
00224                 ODESolver::StateType error (error_.begin (), error_.end ());
00225                 return error;
00226             }
00227 
00228         protected:
00230             virtual void solve (StateType &state, const Control* control, const double duration) const
00231             {
00232                 ODESolver::ODEFunctor odefunc (ode_, control);
00233 
00234                 if (error_.size () != state.size ())
00235                     error_.assign (state.size (), 0.0);
00236 
00237                 Solver solver;
00238                 solver.adjust_size (state);
00239 
00240                 double time = 0.0;
00241                 while (time < duration)
00242                 {
00243                     solver.do_step (odefunc, state, time, intStep_, error_);
00244                     time += intStep_;
00245                 }
00246             }
00247 
00249             mutable ODESolver::StateType error_;
00250         };
00251 
00258         template <class Solver = boost::numeric::omplext_odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
00259         class ODEAdaptiveSolver : public ODESolver
00260         {
00261         public:
00264             ODEAdaptiveSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
00265             {
00266             }
00267 
00269             double getMaximumError (void) const
00270             {
00271                 return maxError_;
00272             }
00273 
00275             void setMaximumError (double error)
00276             {
00277                 maxError_ = error;
00278             }
00279 
00281             double getMaximumEpsilonError (void) const
00282             {
00283                 return maxEpsilonError_;
00284             }
00285 
00287             void setMaximumEpsilonError (double error)
00288             {
00289                 maxEpsilonError_ = error;
00290             }
00291 
00292         protected:
00293 
00298             virtual void solve (StateType &state, const Control* control, const double duration) const
00299             {
00300                 ODESolver::ODEFunctor odefunc (ode_, control);
00301 
00302                 boost::numeric::omplext_odeint::controlled_runge_kutta< Solver > solver (boost::numeric::omplext_odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
00303                 boost::numeric::omplext_odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration, intStep_);
00304             }
00305 
00307             double maxError_;
00308 
00310             double maxEpsilonError_;
00311         };
00312     }
00313 }
00314 
00315 #endif
00316 
00317 #endif
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends