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 ¤t, 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