mbsim  4.0.0
MBSim Kernel
 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  inline 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  if(d.size()!=s.size()) d.resize(s.size(),fmatvec::NONINIT);
57  d=s;
58  }
59  template<typename Dst, typename Src>
60  inline void assign(Dst &d, const Src &s) {
61  if(static_cast<size_t>(d.size())!=static_cast<size_t>(s.size()))
62  d.resize(s.size());
63  std::copy(s.begin(), s.end(), d.begin());
64  }
65 
66  // type to define a boost odeint system concept of type "system"
67  struct ExplicitSystemTag {};
68  // type to define a boost odeint system concept of type "implicit system"
69  struct ImplicitSystemTag {};
70 
71  // Helper class to return a object of the boost odeint system concept of type SystemCategory
72  // giving functions for zd and jacobian.
73  // Declaration.
74  template<class SystemCategory, class ZdFunc, class JacFunc>
76  // Explicit spezialization for boost odeint system concept of type "system"
77  template<class ZdFunc, class JacFunc>
78  class BoostOdeintSystem<ExplicitSystemTag, ZdFunc, JacFunc> {
79  public:
80  BoostOdeintSystem(const ZdFunc& zdFunc__, const JacFunc&) : zdFunc_(zdFunc__) {}
81  const ZdFunc& operator()() const { return zdFunc_; }
82  private:
83  const ZdFunc& zdFunc_;
84  };
85  // Explicit spezialization for boost odeint system concept of type "implicit system"
86  template<class ZdFunc, class JacFunc>
87  class BoostOdeintSystem<ImplicitSystemTag, ZdFunc, JacFunc> {
88  public:
89  BoostOdeintSystem(const ZdFunc& zdFunc__, const JacFunc& jacFunc__) : implFunc_(make_pair(zdFunc__, jacFunc__)) {}
90  const std::pair<ZdFunc, JacFunc>& operator()() const { return implFunc_; }
91  private:
92  const std::pair<ZdFunc, JacFunc> implFunc_;
93  };
94  }
95 
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  size_t plotSample;
166  typename DOS::state_type zTemp;
167 
168  // internal variables required for the numerical jacobian calculation
169  fmatvec::Vec zDisturbed;
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  if(dtPlot<0)
192  system->plot();
193  }
194 
195  template<typename DOS>
196  void BoostOdeintDOS<DOS>::jac(const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac, const double t,
197  typename DOS::state_type &ft) {
198  nrJacs++;
199  // RHS jacobian
200  if(static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size1()) ||
201  static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size2()))
202  jac.resize(z.size(), z.size());
203  system->setTime(t);
204  BoostOdeintHelper::assign(system->getState(), z);
205  system->resetUpToDate();
206  BoostOdeintHelper::assign(zDisturbed, z);
207  fmatvec::Vec zd0=system->evalzd();
208 
209  for(size_t i=0; i<static_cast<size_t>(z.size()); ++i) {
210  zDisturbed(i)+=MBSim::epsroot;
211  system->setState(zDisturbed);
212  system->resetUpToDate();
213  auto col=boost::numeric::ublas::column(jac, i);
214  auto zd=(system->evalzd()-zd0)*MBSim::epsrootInv;
215  std::copy(zd.begin(), zd.end(), col.begin());
216  zDisturbed(i)-=MBSim::epsroot;
217  }
218 
219  system->setTime(t+MBSim::epsroot);
220  system->setState(zDisturbed);
221  system->resetUpToDate();
222  BoostOdeintHelper::assign(ft, (system->evalzd()-zd0)*MBSim::epsrootInv);
223  }
224 
225  template<typename DOS>
227  preIntegrate();
228  subIntegrate(tEnd);
229  postIntegrate();
230  }
231 
232  template<typename DOS>
234  debugInit();
235 
236  nrSteps=0;
237  nrRHS=0;
238  nrJacs=0;
239  nrPlots=0;
240  nrSVs=0;
241  nrRoots=0;
242  nrDriftCorr=0;
243 
244  system->setTime(tStart);
245 
246  // get initial state
247  if(z0.size()) {
248  if(z0.size()!=system->getzSize()+system->getisSize())
249  throwError("BoostOdeintDOS:: size of z0 does not match, must be " + std::to_string(system->getzSize()+system->getisSize()));
250  BoostOdeintHelper::assign(zTemp, z0(fmatvec::RangeV(0,system->getzSize()-1)));
251  system->setInternalState(z0(fmatvec::RangeV(system->getzSize(),z0.size()-1)));
252  }
253  else
254  BoostOdeintHelper::assign(zTemp, system->evalz0());
255 
256  BoostOdeintHelper::assign(system->getState(), zTemp);
257  system->resetUpToDate();
258  system->computeInitialCondition();
259  nrPlots++;
260  system->plot();
261  plotSample=1;
262  tPlot=tStart+plotSample*dtPlot;
263  nrSVs++;
264  svLast <<= system->evalsv();
265  BoostOdeintHelper::assign(zTemp, system->getState()); // needed, as computeInitialCondition may change the state
266 
267  // initialize odeint
268  dos.reset(new DOS(aTol, rTol, dtMax));
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  static constexpr double oneMinusEps = 1 - std::numeric_limits<double>::epsilon();
276  while(dos->current_time()<tSamplePoint*oneMinusEps) {
277  // make one step with odeint
278  nrSteps++;
279 #if BOOST_VERSION >= 107100
280  // do not make a step which exceeds tSamplePoint
281  dos->setDtMax(std::min(dtMax, tSamplePoint-dos->current_time()));
282 #endif
283  auto step=dos->do_step(boostOdeintSystem());
284 
285  // check if a root exists in the current step
286  double curTimeAndState=dos->current_time(); // save current time/state as double: just used to avoid unnessesary system updates
287  system->setTime(dos->current_time());
288  BoostOdeintHelper::assign(system->getState(), dos->current_state());
289  system->resetUpToDate();
290  nrSVs++;
291  shift=signChangedWRTsvLast(system->evalsv());
292  // if a root exists in the current step ...
293  if(shift) {
294  // ... search the first root and set step.second to this time
295  double dt=step.second-step.first;
296  while(dt>dtRoot) {
297  dt/=2;
298  double tRoot=step.second-dt;
299  dos->calc_state(tRoot, zTemp);
300  curTimeAndState=tRoot;
301  system->setTime(tRoot);
302  BoostOdeintHelper::assign(system->getState(), zTemp);
303  system->resetUpToDate();
304  nrSVs++;
305  if(signChangedWRTsvLast(system->evalsv()))
306  step.second=tRoot;
307  }
308  // root found increase time by dt and set jsv
309  step.second+=dt;
310  dos->calc_state(step.second, zTemp);
311  curTimeAndState=step.second;
312  system->setTime(step.second);
313  BoostOdeintHelper::assign(system->getState(), zTemp);
314  system->resetUpToDate();
315  nrSVs++;
316  auto &sv=system->evalsv();
317  auto &jsv=system->getjsv();
318  for(int i=0; i<sv.size(); ++i)
319  jsv(i)=svLast(i)*sv(i)<0;
320  }
321 
322  if(dtPlot>0) {
323  // plot every dtPlot up to the end of the current step (the current step end may already be shorted by roots)
324  static constexpr double onePlusEps = 1 + std::numeric_limits<double>::epsilon();
325  while(tPlot<=step.second*onePlusEps
326 #if BOOST_VERSION < 107100
327  && tPlot<=tSamplePoint*onePlusEps
328 #endif
329  ) {
330  dos->calc_state(tPlot, zTemp);
331  if(curTimeAndState!=tPlot) {
332  curTimeAndState=tPlot;
333  system->setTime(tPlot);
334  BoostOdeintHelper::assign(system->getState(), zTemp);
335  system->resetUpToDate();
336  }
337  nrPlots++;
338  system->plot();
339  if(msgAct(Status))
340  msg(Status)<<"t = "<<tPlot<<", dt="<<dos->current_time_step()<<" "<<std::flush;
341  system->updateInternalState();
342  plotSample++;
343  tPlot=tStart+plotSample*dtPlot;
344  }
345  }
346  else {
347  // for dtPlot<=0 plot every integrator step
348  nrPlots++;
349  system->plot();
350  if(msgAct(Status))
351  msg(Status)<<"t = "<<step.second<<", dt="<<dos->current_time_step()<<" "<<std::flush;
352  }
353 
354 #if BOOST_VERSION < 107100
355  if(step.second<tSamplePoint) {
356 #endif
357  if(shift) {
358  // shift the system
359  dos->calc_state(step.second, zTemp);
360  if(curTimeAndState!=step.second) {
361  curTimeAndState=step.second;
362  system->setTime(step.second);
363  BoostOdeintHelper::assign(system->getState(), zTemp);
364  system->resetUpToDate();
365  }
366  if(plotOnRoot) {
367  nrPlots++;
368  system->plot();
369  }
370  nrRoots++;
371  system->resetUpToDate();
372  system->shift();
373  if(plotOnRoot) {
374  nrPlots++;
375  system->plot();
376  }
377  nrSVs++;
378  svLast=system->evalsv();
379  // reinit odeint with new state
380  BoostOdeintHelper::assign(zTemp, system->getState());
381  dos->initialize(zTemp, step.second, dos->current_time_step());
382  }
383  else {
384  // curTimeAndState may not be a step.second due to plotting -> reset DSS to this time/state if so
385  // (note that due to root finding curTimeAndSTate can also be different to step.second but in this case this code is not reached)
386  if((gMax>=0 || gdMax>=0) && curTimeAndState!=step.second) {
387  curTimeAndState=step.second;
388  system->setTime(step.second);
389  BoostOdeintHelper::assign(system->getState(), dos->current_state());
390  system->resetUpToDate();
391  }
392  // check if projection is needed (if a root was found projection is done anyway by shift())
393  bool reinitNeeded=false;
394  if(gMax>=0 and system->positionDriftCompensationNeeded(gMax)) {
395  system->projectGeneralizedPositions(3);
396  system->projectGeneralizedVelocities(3);
397  reinitNeeded=true;
398  }
399  else if(gdMax>=0 and system->velocityDriftCompensationNeeded(gdMax)) {
400  system->projectGeneralizedVelocities(3);
401  reinitNeeded=true;
402  }
403  if(reinitNeeded) {
404  nrDriftCorr++;
405  BoostOdeintHelper::assign(zTemp, system->getState());
406  dos->initialize(zTemp, step.second, dos->current_time_step());
407  }
408  system->updateStopVectorParameters();
409  }
410 #if BOOST_VERSION < 107100
411  }
412  else {
413  // TODO: this code block (espezially the dos->initialize) should be avoided since initializting the integrator
414  // is expensive and not needed at macro sample points or updates of discrete states.
415  // For this the do_step call at the top must be restricted to reach at most tSamplePoint.
416  // However, this seem currently not possible with the boost odeint integrators.
417  // That's why we use this workaround here which has not the optimal performance.
418  dos->calc_state(tSamplePoint, zTemp);
419  curTimeAndState=tSamplePoint;
420  system->setTime(tSamplePoint);
421  BoostOdeintHelper::assign(system->getState(), zTemp);
422  system->resetUpToDate();
423  dos->initialize(zTemp, tSamplePoint, dos->current_time_step());
424  system->updateStopVectorParameters();
425  }
426 #endif
427 
428 #if BOOST_VERSION >= 107100
429  // if the system time or state was changed after do_step (for plotting, root-finding, ...)
430  // we need to reset it to the integrator state
431  if(curTimeAndState!=dos->current_time()) {
432  system->setTime(dos->current_time());
433  BoostOdeintHelper::assign(system->getState(), dos->current_state());
434  system->resetUpToDate();
435  }
436 #endif
437  system->updateInternalState();
438  }
439  }
440 
441  template<typename DOS>
442  void BoostOdeintDOS<DOS>::postIntegrate() {
443  msg(Info)<<std::endl;
444  msg(Info)<<"Integration statistics:"<<std::endl;
445  msg(Info)<<"nrSteps = "<<nrSteps<<std::endl;
446  msg(Info)<<"nrRHS = "<<nrRHS<<std::endl;
447  msg(Info)<<"nrJacs = "<<nrJacs<<std::endl;
448  msg(Info)<<"nrPlots = "<<nrPlots<<std::endl;
449  msg(Info)<<"nrSVs = "<<nrSVs<<std::endl;
450  msg(Info)<<"nrRoots = "<<nrRoots<<std::endl;
451  msg(Info)<<"nrDriftCorr = "<<nrDriftCorr<<std::endl;
452  }
453 
454  namespace {
455  // declaration
456  template<bool, typename Self>
457  struct InitializeUsingXMLControlled;
458 
459  // initializeUsingXML only used if isControlled == true
460  template<typename Self>
461  struct InitializeUsingXMLControlled<true, Self> {
462  static void call(Self self, xercesc::DOMElement* element) {
463  xercesc::DOMElement *e;
464 
465  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"absoluteToleranceScalar");
466  if(e) self->setAbsoluteTolerance(MBXMLUtils::E(e)->getText<double>());
467 
468  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"relativeToleranceScalar");
469  if(e) self->setRelativeTolerance(MBXMLUtils::E(e)->getText<double>());
470 
471  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"maximumStepSize");
472  if(e) self->setMaximumStepSize(MBXMLUtils::E(e)->getText<double>());
473  }
474  };
475 
476  // initializeUsingXML only used if isControlled == false
477  template<typename Self>
478  struct InitializeUsingXMLControlled<false, Self> {
479  static void call(Self self, xercesc::DOMElement* element) {
480  xercesc::DOMElement *e;
481 
482  // we are not checking in the XML schema for isControlled -> do it here at runtime
483 
484  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"absoluteToleranceScalar");
485  if(e) self->throwError("absoluteToleranceScalar element used for an fixed step-size stepper.");
486 
487  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"relativeToleranceScalar");
488  if(e) self->throwError("relativeToleranceScalar element used for an fixed step-size stepper.");
489 
490  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"maximumStepSize");
491  if(e) self->throwError("maximumStepSize element used for an fixed step-size stepper.");
492  }
493  };
494  }
495 
496  template<typename DOS>
497  void BoostOdeintDOS<DOS>::initializeUsingXML(xercesc::DOMElement *element) {
499  xercesc::DOMElement *e;
500 
501  InitializeUsingXMLControlled<isControlled, decltype(this)>::call(this, element);
502 
503  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"initialStepSize");
504  if(e) setInitialStepSize(MBXMLUtils::E(e)->getText<double>());
505  }
506 
507 }
508 
509 #endif
void setMaximumStepSize(double dtMax_)
Set maximum step size.
Definition: boost_odeint_integrator.h:132
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:69
Definition: boost_odeint_integrator.h:67
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:497
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:226
Definition: boost_odeint_integrator.h:75