All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
boost_odeint_integrator.h
1 /* Copyright (C) 2017 Markus Friedrich
2 
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  */
17 
18 #ifndef _BOOST_ODEINT_INTEGRATOR_H_
19 #define _BOOST_ODEINT_INTEGRATOR_H_
20 
21 #include "root_finding_integrator.h"
22 #include <mbsim/dynamic_system_solver.h>
23 #include <mbsim/utils/eps.h>
24 #include <boost/numeric/odeint/util/is_resizeable.hpp>
25 #include <boost/numeric/odeint/stepper/stepper_categories.hpp>
26 #include <boost/numeric/ublas/matrix.hpp>
27 #include <boost/numeric/ublas/matrix_proxy.hpp>
28 
29 namespace boost {
30  namespace numeric {
31  namespace odeint {
32  // Enable fmatvec::Vec, the state type of MBSim, as a boost odeint state type.
33  template<>
34  struct is_resizeable<fmatvec::Vec> {
35  typedef boost::true_type type;
36  static const bool value=true;
37  };
38  }
39  }
40 
41  // Enable fmatvec::Vec, the state type of MBSim, as a boost odeint state type.
42  int size(const fmatvec::Vec& v) {
43  return v.size();
44  }
45 }
46 
47 namespace MBSim {
48 
49  namespace BoostOdeintHelper {
50 
51  // Convert a MBSim state (fmatvec::Vec) to the boost odeint state type.
52  // Most boost odeint integrators can handle arbitariy state type (see above how fmatvec::Vec is enabled for boost odeint).
53  // In this case the following function are defined as noop.
54  // If boost odeint requires a special state type then the state is copied.
55  inline void assign(fmatvec::Vec &d, const fmatvec::Vec &s) {
56  d=s;
57  }
58  template<typename Dst, typename Src>
59  inline void assign(Dst &d, const Src &s) {
60  if(static_cast<size_t>(d.size())!=static_cast<size_t>(s.size()))
61  d.resize(s.size());
62  copy(s.begin(), s.end(), d.begin());
63  }
64 
65  // type to define a boost odeint system concept of type "system"
66  struct ExplicitSystemTag {};
67  // type to define a boost odeint system concept of type "implicit system"
68  struct ImplicitSystemTag {};
69 
70  // Helper class to return a object of the boost odeint system concept of type SystemCategory
71  // giving functions for zd and jacobian.
72  // Declaration.
73  template<class SystemCategory, class ZdFunc, class JacFunc>
75  // Explicit spezialization for boost odeint system concept of type "system"
76  template<class ZdFunc, class JacFunc>
77  class BoostOdeintSystem<ExplicitSystemTag, ZdFunc, JacFunc> {
78  public:
79  BoostOdeintSystem(const ZdFunc& zdFunc__, const JacFunc&) : zdFunc_(zdFunc__) {}
80  const ZdFunc& operator()() const { return zdFunc_; }
81  private:
82  const ZdFunc& zdFunc_;
83  };
84  // Explicit spezialization for boost odeint system concept of type "implicit system"
85  template<class ZdFunc, class JacFunc>
86  class BoostOdeintSystem<ImplicitSystemTag, ZdFunc, JacFunc> {
87  public:
88  BoostOdeintSystem(const ZdFunc& zdFunc__, const JacFunc& jacFunc__) : implFunc_(make_pair(zdFunc__, jacFunc__)) {}
89  const std::pair<ZdFunc, JacFunc>& operator()() const { return implFunc_; }
90  private:
91  const std::pair<ZdFunc, JacFunc> implFunc_;
92  };
93  }
94 
102  template<typename DOS>
104  protected:
105  // flag if the underlaying stepper is controlled
106  static constexpr bool isControlled{std::is_base_of<boost::numeric::odeint::controlled_stepper_tag,
107  typename DOS::UnderlayingStepperCategory>::value};
108  // helper typedef to enable member functions if the underlaying stepper is controlled
109  template<typename H>
110  using EnableIfControlled=typename std::enable_if<std::is_base_of<boost::numeric::odeint::controlled_stepper_tag,
111  typename H::UnderlayingStepperCategory>::value>::type;
112 
113  public:
114  void integrate() override;
115  void preIntegrate() override;
116  void subIntegrate(double tSamplePoint) override;
117  void postIntegrate() override;
118 
120  void setInitialStepSize(double dt0_) { dt0=dt0_; }
121 
123  template<typename H=DOS, typename=EnableIfControlled<H>>
124  void setAbsoluteTolerance(double aTol_) { aTol=aTol_; }
125 
127  template<typename H=DOS, typename=EnableIfControlled<H>>
128  void setRelativeTolerance(double rTol_) { rTol=rTol_; }
129 
131  template<typename H=DOS, typename=EnableIfControlled<H>>
132  void setMaximumStepSize(double dtMax_) { dtMax=dtMax_; }
133 
134  void initializeUsingXML(xercesc::DOMElement *element) override;
135  protected:
136  // boost odeint style member function calculating zd
137  void zd(const typename DOS::state_type &z, typename DOS::state_type &zd, const double t);
138  // boost odeint style member function calculating the jacobian
139  void jac(const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac, const double t,
140  typename DOS::state_type &ft);
141  // boost odeint style std::function's calculating zd
142  const std::function<void(const typename DOS::state_type&, typename DOS::state_type&, const double)> zdFunc
143  {bind(&BoostOdeintDOS<DOS>::zd, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)};
144  // boost odeint style std::function's calculating the jacobian
145  const std::function<void(const typename DOS::state_type&, boost::numeric::ublas::matrix<double>&c, const double,
146  typename DOS::state_type&)> jacFunc
147  {bind(&BoostOdeintDOS<DOS>::jac, this, std::placeholders::_1, std::placeholders::_2,
148  std::placeholders::_3, std::placeholders::_4)};
149  // Helper member which returns a object of the boost odeint system concept of type SystemCategory
150  // giving functions for zd and jacobian.
151  const BoostOdeintHelper::BoostOdeintSystem<typename DOS::SystemCategory, decltype(zdFunc), decltype(jacFunc)>
152  boostOdeintSystem{zdFunc, jacFunc};
153 
154  // the boost odeint dense output stepper.
155  std::unique_ptr<DOS> dos;
156 
157  // variables with corresponding setter functions
158  double dt0{1e-10};
159  double aTol{1e-6};
160  double rTol{1e-6};
161  double dtMax{0.1};
162 
163  // internal variables
164  double tPlot;
165  typename DOS::state_type zTemp;
166 
167  // internal variables required for the numerical jacobian calculation
168  fmatvec::Vec zDisturbed;
169  fmatvec::Vec zd0;
170 
171  // internal variables used for integrator statistics
172  size_t nrSteps;
173  size_t nrRHS;
174  size_t nrJacs;
175  size_t nrPlots;
176  size_t nrSVs;
177  size_t nrRoots;
178  size_t nrDriftCorr;
179  };
180 
181  // implementation
182 
183  template<typename DOS>
184  void BoostOdeintDOS<DOS>::zd(const typename DOS::state_type &z, typename DOS::state_type &zd, const double t) {
185  nrRHS++;
186  // RHS
187  system->setTime(t);
188  BoostOdeintHelper::assign(system->getState(), z);
189  system->resetUpToDate();
190  BoostOdeintHelper::assign(zd, system->evalzd());
191  }
192 
193  template<typename DOS>
194  void BoostOdeintDOS<DOS>::jac(const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac, const double t,
195  typename DOS::state_type &ft) {
196  nrJacs++;
197  // RHS jacobian
198  if(static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size1()) ||
199  static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size2()))
200  jac.resize(z.size(), z.size());
201  system->setTime(t);
202  BoostOdeintHelper::assign(system->getState(), z);
203  system->resetUpToDate();
204  BoostOdeintHelper::assign(zDisturbed, z);
205  zd0=system->evalzd();
206 
207  for(size_t i=0; i<static_cast<size_t>(z.size()); ++i) {
208  zDisturbed(i)+=MBSim::epsroot;
209  system->setState(zDisturbed);
210  system->resetUpToDate();
211  auto col=boost::numeric::ublas::column(jac, i);
212  auto zd=(system->evalzd()-zd0)*MBSim::epsrootInv;
213  copy(zd.begin(), zd.end(), col.begin());
214  zDisturbed(i)-=MBSim::epsroot;
215  }
216 
217  system->setTime(t+MBSim::epsroot);
218  system->setState(zDisturbed);
219  system->resetUpToDate();
220  BoostOdeintHelper::assign(ft, (system->evalzd()-zd0)*MBSim::epsrootInv);
221  }
222 
223  template<typename DOS>
225  preIntegrate();
226  subIntegrate(tEnd);
227  postIntegrate();
228  }
229 
230  template<typename DOS>
232  debugInit();
233 
234  nrSteps=0;
235  nrRHS=0;
236  nrJacs=0;
237  nrPlots=0;
238  nrSVs=0;
239  nrRoots=0;
240  nrDriftCorr=0;
241 
242  system->setTime(tStart);
243 
244  // get initial state
245  if(z0.size()) {
246  if(z0.size()!=system->getzSize())
247  throwError("BoostOdeintDOS:: size of z0 does not match, must be " + std::to_string(system->getzSize()));
248  BoostOdeintHelper::assign(zTemp, z0);
249  }
250  else
251  BoostOdeintHelper::assign(zTemp, system->evalz0());
252 
253  system->resetUpToDate();
254  system->computeInitialCondition();
255  nrPlots++;
256  system->plot();
257  tPlot=tStart+dtPlot;
258  nrSVs++;
259  svLast=system->evalsv();
260 
261  // initialize odeint
262 #if BOOST_VERSION >= 106000
263  dos.reset(new DOS(aTol, rTol, dtMax));
264 #else // boost odeint < 1.60 does not support dtMax
265  dos.reset(new DOS(aTol, rTol));
266  msg(Warn)<<"This build was done with boost < 1.60 which does not support a maximum step size."<<std::endl
267  <<"Integrator will not limit the maximum step size."<<std::endl;
268 #endif
269  dos->initialize(zTemp, tStart, dt0);
270  }
271 
272  template<typename DOS>
273  void BoostOdeintDOS<DOS>::subIntegrate(double tSamplePoint) {
274  // loop until at least tSamplePoint is reached
275  while(dos->current_time()<tSamplePoint) {
276  // make one step with odeint
277  nrSteps++;
278  auto step=dos->do_step(boostOdeintSystem());
279 
280  // check if a root exists in the current step
281  double curTimeAndState=dos->current_time(); // save current time/state as double: just used to avoid unnessesary system updates
282  system->setTime(dos->current_time());
283  BoostOdeintHelper::assign(system->getState(), dos->current_state());
284  system->resetUpToDate();
285  nrSVs++;
286  shift=signChangedWRTsvLast(system->evalsv());
287  // if a root exists in the current step ...
288  if(shift) {
289  // ... search the first root and set step.second to this time
290  double dt=step.second-step.first;
291  while(dt>dtRoot) {
292  dt/=2;
293  double tRoot=step.second-dt;
294  dos->calc_state(tRoot, zTemp);
295  curTimeAndState=tRoot;
296  system->setTime(tRoot);
297  BoostOdeintHelper::assign(system->getState(), zTemp);
298  system->resetUpToDate();
299  nrSVs++;
300  if(signChangedWRTsvLast(system->evalsv()))
301  step.second=tRoot;
302  }
303  // root found increase time by dt and set jsv
304  step.second+=dt;
305  dos->calc_state(step.second, zTemp);
306  curTimeAndState=step.second;
307  system->setTime(step.second);
308  BoostOdeintHelper::assign(system->getState(), zTemp);
309  system->resetUpToDate();
310  nrSVs++;
311  auto &sv=system->evalsv();
312  auto &jsv=system->getjsv();
313  for(int i=0; i<sv.size(); ++i)
314  jsv(i)=svLast(i)*sv(i)<0;
315  }
316 
317  // plot every dtPlot up to the end of the current step (the current step end may already be shorted by roots)
318  while(tPlot<=step.second && tPlot<tSamplePoint) {
319  dos->calc_state(tPlot, zTemp);
320  if(curTimeAndState!=tPlot) {
321  curTimeAndState=tPlot;
322  system->setTime(tPlot);
323  BoostOdeintHelper::assign(system->getState(), zTemp);
324  system->resetUpToDate();
325  }
326  nrPlots++;
327  system->plot();
328  if(msgAct(Status))
329  msg(Status)<<"t = "<<tPlot<<", dt="<<dos->current_time_step()<<std::flush;
330  tPlot+=dtPlot;
331  }
332 
333  if(step.second<tSamplePoint) {
334  if(shift) {
335  // shift the system
336  dos->calc_state(step.second, zTemp);
337  if(curTimeAndState!=step.second) {
338  curTimeAndState=step.second;
339  system->setTime(step.second);
340  BoostOdeintHelper::assign(system->getState(), zTemp);
341  system->resetUpToDate();
342  }
343  if(plotOnRoot) {
344  nrPlots++;
345  system->plot();
346  }
347  nrRoots++;
348  system->resetUpToDate();
349  system->shift();
350  if(plotOnRoot) {
351  nrPlots++;
352  system->plot();
353  }
354  nrSVs++;
355  svLast=system->evalsv();
356  // reinit odeint with new state
357  BoostOdeintHelper::assign(zTemp, system->getState());
358  dos->initialize(zTemp, step.second, dos->current_time_step());
359  }
360  else {
361  // check if projection is needed (if a root was found projection is done anyway by shift())
362  bool reinitNeeded=false;
363  if(gMax>=0 and system->positionDriftCompensationNeeded(gMax)) {
364  system->projectGeneralizedPositions(3);
365  system->projectGeneralizedVelocities(3);
366  reinitNeeded=true;
367  }
368  else if(gdMax>=0 and system->velocityDriftCompensationNeeded(gdMax)) {
369  system->projectGeneralizedVelocities(3);
370  reinitNeeded=true;
371  }
372  if(reinitNeeded) {
373  nrDriftCorr++;
374  BoostOdeintHelper::assign(zTemp, system->getState());
375  dos->initialize(zTemp, step.second, dos->current_time_step());
376  }
377  system->updateStopVectorParameters();
378  }
379  }
380  else {
381  // TODO: this code block (espezially the dos->initialize) should be avoided since initializting the integrator
382  // is expensive and not needed at macro sample points or updates of discrete states.
383  // For this the do_step call at the top must be restricted to reach at most tSamplePoint.
384  // However, this seem currently not possible with the boost odeint integrators.
385  // That's why we use this workaround here which has not the optimal performance.
386  dos->calc_state(tSamplePoint, zTemp);
387  curTimeAndState=tSamplePoint;
388  system->setTime(tSamplePoint);
389  BoostOdeintHelper::assign(system->getState(), zTemp);
390  dos->initialize(zTemp, tSamplePoint, dos->current_time_step());
391  system->updateStopVectorParameters();
392  }
393  }
394  }
395 
396  template<typename DOS>
397  void BoostOdeintDOS<DOS>::postIntegrate() {
398  msg(Info)<<std::endl;
399  msg(Info)<<"Integration statistics:"<<std::endl;
400  msg(Info)<<"nrSteps = "<<nrSteps<<std::endl;
401  msg(Info)<<"nrRHS = "<<nrRHS<<std::endl;
402  msg(Info)<<"nrJacs = "<<nrJacs<<std::endl;
403  msg(Info)<<"nrPlots = "<<nrPlots<<std::endl;
404  msg(Info)<<"nrSVs = "<<nrSVs<<std::endl;
405  msg(Info)<<"nrRoots = "<<nrRoots<<std::endl;
406  msg(Info)<<"nrDriftCorr = "<<nrDriftCorr<<std::endl;
407  }
408 
409  namespace {
410  // declaration
411  template<bool, typename Self>
412  struct InitializeUsingXMLControlled;
413 
414  // initializeUsingXML only used if isControlled == true
415  template<typename Self>
416  struct InitializeUsingXMLControlled<true, Self> {
417  static void call(Self self, xercesc::DOMElement* element) {
418  xercesc::DOMElement *e;
419 
420  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"absoluteToleranceScalar");
421  if(e) self->setAbsoluteTolerance(MBXMLUtils::E(e)->getText<double>());
422 
423  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"relativeToleranceScalar");
424  if(e) self->setRelativeTolerance(MBXMLUtils::E(e)->getText<double>());
425 
426  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"maximumStepSize");
427  if(e) self->setMaximumStepSize(MBXMLUtils::E(e)->getText<double>());
428  }
429  };
430 
431  // initializeUsingXML only used if isControlled == false
432  template<typename Self>
433  struct InitializeUsingXMLControlled<false, Self> {
434  static void call(Self self, xercesc::DOMElement* element) {
435  xercesc::DOMElement *e;
436 
437  // we are not checking in the XML schema for isControlled -> do it here at runtime
438 
439  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"absoluteToleranceScalar");
440  if(e) self->throwError("absoluteToleranceScalar element used for an fixed step-size stepper.");
441 
442  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"relativeToleranceScalar");
443  if(e) self->throwError("relativeToleranceScalar element used for an fixed step-size stepper.");
444 
445  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"maximumStepSize");
446  if(e) self->throwError("maximumStepSize element used for an fixed step-size stepper.");
447  }
448  };
449  }
450 
451  template<typename DOS>
452  void BoostOdeintDOS<DOS>::initializeUsingXML(xercesc::DOMElement *element) {
454  xercesc::DOMElement *e;
455 
456  InitializeUsingXMLControlled<isControlled, decltype(this)>::call(this, element);
457 
458  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"initialStepSize");
459  if(e) setInitialStepSize(MBXMLUtils::E(e)->getText<double>());
460  }
461 
462 }
463 
464 #endif
void setMaximumStepSize(double dtMax_)
Set maximum step size.
Definition: boost_odeint_integrator.h:132
casadi::DM c(const double &x)
convert double to casadi::DM
Definition: symbolic_function.h:93
Integrator with root-finding.
Definition: root_finding_integrator.h:32
void setRelativeTolerance(double rTol_)
Set relative tolerance.
Definition: boost_odeint_integrator.h:128
Definition: boost_odeint_integrator.h:68
Definition: boost_odeint_integrator.h:66
void setInitialStepSize(double dt0_)
Set initial step size.
Definition: boost_odeint_integrator.h:120
Definition: boost_odeint_integrator.h:103
void initializeUsingXML(xercesc::DOMElement *element) override
initialize integrator
Definition: boost_odeint_integrator.h:452
virtual void initializeUsingXML(xercesc::DOMElement *element)
initialize integrator
Definition: root_finding_integrator.cc:44
void setAbsoluteTolerance(double aTol_)
Set absolute tolerance.
Definition: boost_odeint_integrator.h:124
void integrate() override
start the integration of the system set by setSystem. Each class implemeting this function should cal...
Definition: boost_odeint_integrator.h:224
Definition: boost_odeint_integrator.h:74

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Home