ompl/control/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 #include "ompl/control/Control.h"
00041 #include "ompl/control/SpaceInformation.h"
00042 #include "ompl/control/StatePropagator.h"
00043 #include "ompl/util/Console.h"
00044 #include "ompl/util/ClassForward.h"
00045 
00046 #include <boost/version.hpp>
00047 #if BOOST_VERSION >= 105300
00048 #include <boost/numeric/odeint.hpp>
00049 namespace odeint = boost::numeric::odeint;
00050 #else
00051 #include <omplext_odeint/boost/numeric/odeint.hpp>
00052 namespace odeint = boost::numeric::omplext_odeint;
00053 #endif
00054 #include <boost/function.hpp>
00055 #include <cassert>
00056 #include <vector>
00057 
00058 namespace ompl
00059 {
00060 
00061     namespace control
00062     {
00063 
00065         OMPL_CLASS_FORWARD(ODESolver);
00067 
00070 
00075         class ODESolver
00076         {
00077         public:
00079             typedef std::vector<double> StateType;
00080 
00083             typedef boost::function<void(const StateType &, const Control*, StateType &)> ODE;
00084 
00087             typedef boost::function<void(const base::State *state, const Control *control, const double duration, base::State *result)> PostPropagationEvent;
00088 
00091             ODESolver (const SpaceInformationPtr& si, const ODE& ode, double intStep) : si_(si), ode_(ode), intStep_(intStep)
00092             {
00093             }
00094 
00096             virtual ~ODESolver ()
00097             {
00098             }
00099 
00101             void setODE (const ODE &ode)
00102             {
00103                 ode_ = ode;
00104             }
00105 
00107             double getIntegrationStepSize () const
00108             {
00109                 return intStep_;
00110             }
00111 
00113             void setIntegrationStepSize (double intStep)
00114             {
00115                 intStep_ = intStep;
00116             }
00117 
00119             const SpaceInformationPtr& getSpaceInformation() const
00120             {
00121                 return si_;
00122             }
00123 
00129             static StatePropagatorPtr getStatePropagator (ODESolverPtr solver,
00130                 const PostPropagationEvent &postEvent = NULL)
00131             {
00132                 class ODESolverStatePropagator : public StatePropagator
00133                 {
00134                     public:
00135                         ODESolverStatePropagator (ODESolverPtr solver, const PostPropagationEvent &pe) : StatePropagator (solver->si_), solver_(solver), postEvent_(pe)
00136                         {
00137                             if (!solver.get())
00138                                 OMPL_ERROR("ODESolverPtr does not reference a valid ODESolver object");
00139                         }
00140 
00141                         virtual void propagate (const base::State *state, const Control *control, const double duration, base::State *result) const
00142                         {
00143                             ODESolver::StateType reals;
00144                             si_->getStateSpace()->copyToReals(reals, state);
00145                             solver_->solve (reals, control, duration);
00146                             si_->getStateSpace()->copyFromReals(result, reals);
00147 
00148                             if (postEvent_)
00149                                 postEvent_ (state, control, duration, result);
00150                         }
00151 
00152                     protected:
00153                         ODESolverPtr solver_;
00154                         ODESolver::PostPropagationEvent postEvent_;
00155                 };
00156                 return StatePropagatorPtr(dynamic_cast<StatePropagator*>(new ODESolverStatePropagator(solver, postEvent)));
00157             }
00158 
00159         protected:
00160 
00162             virtual void solve (StateType &state, const Control *control, const double duration) const = 0;
00163 
00165             const SpaceInformationPtr     si_;
00166 
00168             ODE                           ode_;
00169 
00171             double                        intStep_;
00172 
00174             // Functor used by the boost::numeric::odeint stepper object
00175             struct ODEFunctor
00176             {
00177                 ODEFunctor (const ODE &o, const Control *ctrl) : ode(o), control(ctrl) {}
00178 
00179                 // boost::numeric::odeint will callback to this method during integration to evaluate the system
00180                 void operator () (const StateType &current, StateType &output, double /*time*/)
00181                 {
00182                     ode (current, control, output);
00183                 }
00184 
00185                 ODE ode;
00186                 const Control *control;
00187             };
00189         };
00190 
00197         template <class Solver = odeint::runge_kutta4<ODESolver::StateType> >
00198         class ODEBasicSolver : public ODESolver
00199         {
00200         public:
00201 
00204             ODEBasicSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
00205             {
00206             }
00207 
00208         protected:
00209 
00211             virtual void solve (StateType &state, const Control *control, const double duration) const
00212             {
00213                 Solver solver;
00214                 ODESolver::ODEFunctor odefunc (ode_, control);
00215                 odeint::integrate_const (solver, odefunc, state, 0.0, duration, intStep_);
00216             }
00217         };
00218 
00225         template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
00226         class ODEErrorSolver : public ODESolver
00227         {
00228         public:
00231             ODEErrorSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep)
00232             {
00233             }
00234 
00236             ODESolver::StateType getError ()
00237             {
00238                 return error_;
00239             }
00240 
00241         protected:
00243             virtual void solve (StateType &state, const Control *control, const double duration) const
00244             {
00245                 ODESolver::ODEFunctor odefunc (ode_, control);
00246 
00247                 if (error_.size () != state.size ())
00248                     error_.assign (state.size (), 0.0);
00249 
00250                 Solver solver;
00251                 solver.adjust_size (state);
00252 
00253                 double time = 0.0;
00254                 while (time < duration + std::numeric_limits<float>::epsilon())
00255                 {
00256                     solver.do_step (odefunc, state, time, intStep_, error_);
00257                     time += intStep_;
00258                 }
00259             }
00260 
00262             mutable ODESolver::StateType error_;
00263         };
00264 
00271         template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType> >
00272         class ODEAdaptiveSolver : public ODESolver
00273         {
00274         public:
00277             ODEAdaptiveSolver (const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2) : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
00278             {
00279             }
00280 
00282             double getMaximumError () const
00283             {
00284                 return maxError_;
00285             }
00286 
00288             void setMaximumError (double error)
00289             {
00290                 maxError_ = error;
00291             }
00292 
00294             double getMaximumEpsilonError () const
00295             {
00296                 return maxEpsilonError_;
00297             }
00298 
00300             void setMaximumEpsilonError (double error)
00301             {
00302                 maxEpsilonError_ = error;
00303             }
00304 
00305         protected:
00306 
00311             virtual void solve (StateType &state, const Control *control, const double duration) const
00312             {
00313                 ODESolver::ODEFunctor odefunc (ode_, control);
00314 
00315 #if BOOST_VERSION < 105600
00316                 odeint::controlled_runge_kutta< Solver > solver (odeint::default_error_checker<double>(maxError_, maxEpsilonError_));
00317 #else
00318                 typename boost::numeric::odeint::result_of::make_controlled< Solver >::type solver = make_controlled( 1.0e-6 , 1.0e-6 , Solver() );
00319 #endif
00320                 odeint::integrate_adaptive (solver, odefunc, state, 0.0, duration, intStep_);
00321             }
00322 
00324             double maxError_;
00325 
00327             double maxEpsilonError_;
00328         };
00329     }
00330 }
00331 
00332 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines