Loading...
Searching...
No Matches
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#include "ompl/control/Control.h"
41#include "ompl/control/SpaceInformation.h"
42#include "ompl/control/StatePropagator.h"
43#include "ompl/util/Console.h"
44#include "ompl/util/ClassForward.h"
45
46#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
47#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
48#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
49#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
50#include <boost/numeric/odeint/integrate/integrate_const.hpp>
51#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
52namespace odeint = boost::numeric::odeint;
53#include <functional>
54#include <cassert>
55#include <utility>
56#include <vector>
57
58namespace ompl
59{
60 namespace control
61 {
63 OMPL_CLASS_FORWARD(ODESolver);
65
68
74 {
75 public:
77 using StateType = std::vector<double>;
78
81 using ODE = std::function<void(const StateType &, const Control *, StateType &)>;
82
86 std::function<void(const base::State *, const Control *, double, base::State *)>;
87
90 ODESolver(SpaceInformationPtr si, ODE ode, double intStep)
91 : si_(std::move(si)), ode_(std::move(ode)), intStep_(intStep)
92 {
93 }
94
96 virtual ~ODESolver() = default;
97
99 void setODE(const ODE &ode)
100 {
101 ode_ = ode;
102 }
103
106 {
107 return intStep_;
108 }
109
111 void setIntegrationStepSize(double intStep)
112 {
113 intStep_ = intStep;
114 }
115
118 {
119 return si_;
120 }
121
128 const PostPropagationEvent &postEvent = nullptr)
129 {
130 class ODESolverStatePropagator : public StatePropagator
131 {
132 public:
133 ODESolverStatePropagator(const ODESolverPtr& solver, PostPropagationEvent pe)
134 : StatePropagator(solver->si_), solver_(solver), postEvent_(std::move(pe))
135 {
136 if (solver == nullptr)
137 OMPL_ERROR("ODESolverPtr does not reference a valid ODESolver object");
138 }
139
140 void propagate(const base::State *state, const Control *control, double duration,
141 base::State *result) const override
142 {
144 si_->getStateSpace()->copyToReals(reals, state);
145 solver_->solve(reals, control, duration);
146 si_->getStateSpace()->copyFromReals(result, reals);
147
148 if (postEvent_)
149 postEvent_(state, control, duration, result);
150 }
151
152 protected:
153 ODESolverPtr solver_;
155 };
156 return std::make_shared<ODESolverStatePropagator>(std::move(solver), postEvent);
157 }
158
159 protected:
161 virtual void solve(StateType &state, const Control *control, 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(ODE o, const Control *ctrl) : ode(std::move(o)), control(ctrl)
177 {
178 }
179
180 // boost::numeric::odeint will callback to this method during integration to evaluate the system
181 void operator()(const StateType &current, StateType &output, double /*time*/)
182 {
183 ode(current, control, output);
184 }
185
186 ODE ode;
187 const Control *control;
188 };
190 };
191
198 template <class Solver = odeint::runge_kutta4<ODESolver::StateType>>
200 {
201 public:
204 ODEBasicSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
205 : ODESolver(si, ode, intStep)
206 {
207 }
208
209 protected:
211 void solve(StateType &state, const Control *control, double duration) const override
212 {
213 Solver solver;
214 ODESolver::ODEFunctor odefunc(ode_, control);
215 odeint::integrate_const(solver, odefunc, state, 0.0, duration, intStep_);
216 }
217 };
218
225 template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType>>
227 {
228 public:
231 ODEErrorSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
232 : ODESolver(si, ode, intStep)
233 {
234 }
235
238 {
239 return error_;
240 }
241
242 protected:
244 void solve(StateType &state, const Control *control, double duration) const override
245 {
246 ODESolver::ODEFunctor odefunc(ode_, control);
247
248 if (error_.size() != state.size())
249 error_.assign(state.size(), 0.0);
250
251 Solver solver;
252 solver.adjust_size(state);
253
254 double time = 0.0;
255 while (time < duration + std::numeric_limits<float>::epsilon())
256 {
257 solver.do_step(odefunc, state, time, intStep_, error_);
258 time += intStep_;
259 }
260 }
261
264 };
265
272 template <class Solver = odeint::runge_kutta_cash_karp54<ODESolver::StateType>>
274 {
275 public:
278 ODEAdaptiveSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep = 1e-2)
279 : ODESolver(si, ode, intStep), maxError_(1e-6), maxEpsilonError_(1e-7)
280 {
281 }
282
284 double getMaximumError() const
285 {
286 return maxError_;
287 }
288
290 void setMaximumError(double error)
291 {
292 maxError_ = error;
293 }
294
297 {
298 return maxEpsilonError_;
299 }
300
302 void setMaximumEpsilonError(double error)
303 {
304 maxEpsilonError_ = error;
305 }
306
307 protected:
312 void solve(StateType &state, const Control *control, double duration) const override
313 {
314 ODESolver::ODEFunctor odefunc(ode_, control);
315 auto solver = make_controlled(1.0e-6, 1.0e-6, Solver());
316 odeint::integrate_adaptive(solver, odefunc, state, 0.0, duration, intStep_);
317 }
318
320 double maxError_;
321
324 };
325 }
326}
327
328#endif
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::control::StatePropagator.
Definition of an abstract state.
Definition: State.h:50
Definition of an abstract control.
Definition: Control.h:48
Adaptive step size solver for ordinary differential equations of the type q' = f(q,...
Definition: ODESolver.h:274
double maxError_
The maximum error allowed when performing numerical integration.
Definition: ODESolver.h:320
void solve(StateType &state, const Control *control, double duration) const override
Solve the ordinary differential equation given the input state of the system, a control to apply to t...
Definition: ODESolver.h:312
double getMaximumEpsilonError() const
Retrieve the error tolerance during one step of numerical integration (local truncation error)
Definition: ODESolver.h:296
double maxEpsilonError_
The maximum error allowed during one step of numerical integration.
Definition: ODESolver.h:323
void setMaximumEpsilonError(double error)
Set the error tolerance during one step of numerical integration (local truncation error)
Definition: ODESolver.h:302
void setMaximumError(double error)
Set the total error allowed during numerical integration.
Definition: ODESolver.h:290
double getMaximumError() const
Retrieve the total error allowed during numerical integration.
Definition: ODESolver.h:284
ODEAdaptiveSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:278
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:200
ODEBasicSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:204
void solve(StateType &state, const Control *control, double duration) const override
Solve the ODE using boost::numeric::odeint.
Definition: ODESolver.h:211
Solver for ordinary differential equations of the type q' = f(q, u), where q is the current state of ...
Definition: ODESolver.h:227
ODEErrorSolver(const SpaceInformationPtr &si, const ODESolver::ODE &ode, double intStep=1e-2)
Parameterized constructor. Takes a reference to the SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:231
ODESolver::StateType getError()
Retrieves the error values from the most recent integration.
Definition: ODESolver.h:237
ODESolver::StateType error_
The error values calculated during numerical integration.
Definition: ODESolver.h:263
void solve(StateType &state, const Control *control, double duration) const override
Solve the ODE using boost::numeric::odeint. Save the resulting error values into error_.
Definition: ODESolver.h:244
A shared pointer wrapper for ompl::control::ODESolver.
Abstract base class for an object that can solve ordinary differential equations (ODE) of the type q'...
Definition: ODESolver.h:74
double intStep_
The size of the numerical integration step. Should be small to minimize error.
Definition: ODESolver.h:170
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: ODESolver.h:117
ODE ode_
Definition of the ODE to find solutions for.
Definition: ODESolver.h:167
virtual void solve(StateType &state, const Control *control, double duration) const =0
Solve the ODE given the initial state, and a control to apply for some duration.
std::function< void(const StateType &, const Control *, StateType &)> ODE
Callback function that defines the ODE. Accepts the current state, input control, and output state.
Definition: ODESolver.h:81
virtual ~ODESolver()=default
Destructor.
const SpaceInformationPtr si_
The SpaceInformation that this ODESolver operates in.
Definition: ODESolver.h:164
void setODE(const ODE &ode)
Set the ODE to solve.
Definition: ODESolver.h:99
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:77
double getIntegrationStepSize() const
Return the size of a single numerical integration step.
Definition: ODESolver.h:105
void setIntegrationStepSize(double intStep)
Set the size of a single numerical integration step.
Definition: ODESolver.h:111
static StatePropagatorPtr getStatePropagator(ODESolverPtr solver, const PostPropagationEvent &postEvent=nullptr)
Retrieve a StatePropagator object that solves a system of ordinary differential equations defined by ...
Definition: ODESolver.h:127
std::function< void(const base::State *, const Control *, double, base::State *)> PostPropagationEvent
Callback function to perform an event at the end of numerical integration. This functionality is opti...
Definition: ODESolver.h:86
ODESolver(SpaceInformationPtr si, ODE ode, double intStep)
Parameterized constructor. Takes a reference to SpaceInformation, an ODE to solve,...
Definition: ODESolver.h:90
Model the effect of controls on system states.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Main namespace. Contains everything in this library.
STL namespace.