mbsim  4.0.0
MBSim Kernel
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  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  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  typename DOS::state_type zTemp;
166 
167  // internal variables required for the numerical jacobian calculation
168  fmatvec::Vec zDisturbed;
169 
170  // internal variables used for integrator statistics
171  size_t nrSteps;
172  size_t nrRHS;
173  size_t nrJacs;
174  size_t nrPlots;
175  size_t nrSVs;
176  size_t nrRoots;
177  size_t nrDriftCorr;
178  };
179 
180  // implementation
181 
182  template<typename DOS>
183  void BoostOdeintDOS<DOS>::zd(const typename DOS::state_type &z, typename DOS::state_type &zd, const double t) {
184  nrRHS++;
185  // RHS
186  system->setTime(t);
187  BoostOdeintHelper::assign(system->getState(), z);
188  system->resetUpToDate();
189  BoostOdeintHelper::assign(zd, system->evalzd());
190  }
191 
192  template<typename DOS>
193  void BoostOdeintDOS<DOS>::jac(const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac, const double t,
194  typename DOS::state_type &ft) {
195  nrJacs++;
196  // RHS jacobian
197  if(static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size1()) ||
198  static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size2()))
199  jac.resize(z.size(), z.size());
200  system->setTime(t);
201  BoostOdeintHelper::assign(system->getState(), z);
202  system->resetUpToDate();
203  BoostOdeintHelper::assign(zDisturbed, z);
204  fmatvec::Vec zd0=system->evalzd();
205 
206  for(size_t i=0; i<static_cast<size_t>(z.size()); ++i) {
207  zDisturbed(i)+=MBSim::epsroot;
208  system->setState(zDisturbed);
209  system->resetUpToDate();
210  auto col=boost::numeric::ublas::column(jac, i);
211  auto zd=(system->evalzd()-zd0)*MBSim::epsrootInv;
212  copy(zd.begin(), zd.end(), col.begin());
213  zDisturbed(i)-=MBSim::epsroot;
214  }
215 
216  system->setTime(t+MBSim::epsroot);
217  system->setState(zDisturbed);
218  system->resetUpToDate();
219  BoostOdeintHelper::assign(ft, (system->evalzd()-zd0)*MBSim::epsrootInv);
220  }
221 
222  template<typename DOS>
224  preIntegrate();
225  subIntegrate(tEnd);
226  postIntegrate();
227  }
228 
229  template<typename DOS>
231  debugInit();
232 
233  nrSteps=0;
234  nrRHS=0;
235  nrJacs=0;
236  nrPlots=0;
237  nrSVs=0;
238  nrRoots=0;
239  nrDriftCorr=0;
240 
241  system->setTime(tStart);
242 
243  // get initial state
244  if(z0.size()) {
245  if(z0.size()!=system->getzSize())
246  throwError("BoostOdeintDOS:: size of z0 does not match, must be " + std::to_string(system->getzSize()));
247  BoostOdeintHelper::assign(zTemp, z0);
248  }
249  else
250  BoostOdeintHelper::assign(zTemp, system->evalz0());
251 
252  system->resetUpToDate();
253  system->computeInitialCondition();
254  nrPlots++;
255  system->plot();
256  tPlot=tStart+dtPlot;
257  nrSVs++;
258  svLast <<= system->evalsv();
259 
260  // initialize odeint
261  dos.reset(new DOS(aTol, rTol, dtMax));
262  dos->initialize(zTemp, tStart, dt0);
263  }
264 
265  template<typename DOS>
266  void BoostOdeintDOS<DOS>::subIntegrate(double tSamplePoint) {
267  // loop until at least tSamplePoint is reached
268  while(dos->current_time()<tSamplePoint) {
269  // make one step with odeint
270  nrSteps++;
271  auto step=dos->do_step(boostOdeintSystem());
272 
273  // check if a root exists in the current step
274  double curTimeAndState=dos->current_time(); // save current time/state as double: just used to avoid unnessesary system updates
275  system->setTime(dos->current_time());
276  BoostOdeintHelper::assign(system->getState(), dos->current_state());
277  system->resetUpToDate();
278  nrSVs++;
279  shift=signChangedWRTsvLast(system->evalsv());
280  // if a root exists in the current step ...
281  if(shift) {
282  // ... search the first root and set step.second to this time
283  double dt=step.second-step.first;
284  while(dt>dtRoot) {
285  dt/=2;
286  double tRoot=step.second-dt;
287  dos->calc_state(tRoot, zTemp);
288  curTimeAndState=tRoot;
289  system->setTime(tRoot);
290  BoostOdeintHelper::assign(system->getState(), zTemp);
291  system->resetUpToDate();
292  nrSVs++;
293  if(signChangedWRTsvLast(system->evalsv()))
294  step.second=tRoot;
295  }
296  // root found increase time by dt and set jsv
297  step.second+=dt;
298  dos->calc_state(step.second, zTemp);
299  curTimeAndState=step.second;
300  system->setTime(step.second);
301  BoostOdeintHelper::assign(system->getState(), zTemp);
302  system->resetUpToDate();
303  nrSVs++;
304  auto &sv=system->evalsv();
305  auto &jsv=system->getjsv();
306  for(int i=0; i<sv.size(); ++i)
307  jsv(i)=svLast(i)*sv(i)<0;
308  }
309 
310  // plot every dtPlot up to the end of the current step (the current step end may already be shorted by roots)
311  while(tPlot<=step.second && tPlot<tSamplePoint) {
312  dos->calc_state(tPlot, zTemp);
313  if(curTimeAndState!=tPlot) {
314  curTimeAndState=tPlot;
315  system->setTime(tPlot);
316  BoostOdeintHelper::assign(system->getState(), zTemp);
317  system->resetUpToDate();
318  }
319  nrPlots++;
320  system->plot();
321  if(msgAct(Status))
322  msg(Status)<<"t = "<<tPlot<<", dt="<<dos->current_time_step()<<std::flush;
323  tPlot+=dtPlot;
324  }
325 
326  if(step.second<tSamplePoint) {
327  if(shift) {
328  // shift the system
329  dos->calc_state(step.second, zTemp);
330  if(curTimeAndState!=step.second) {
331  curTimeAndState=step.second;
332  system->setTime(step.second);
333  BoostOdeintHelper::assign(system->getState(), zTemp);
334  system->resetUpToDate();
335  }
336  if(plotOnRoot) {
337  nrPlots++;
338  system->plot();
339  }
340  nrRoots++;
341  system->resetUpToDate();
342  system->shift();
343  if(plotOnRoot) {
344  nrPlots++;
345  system->plot();
346  }
347  nrSVs++;
348  svLast=system->evalsv();
349  // reinit odeint with new state
350  BoostOdeintHelper::assign(zTemp, system->getState());
351  dos->initialize(zTemp, step.second, dos->current_time_step());
352  }
353  else {
354  // check if projection is needed (if a root was found projection is done anyway by shift())
355  bool reinitNeeded=false;
356  if(gMax>=0 and system->positionDriftCompensationNeeded(gMax)) {
357  system->projectGeneralizedPositions(3);
358  system->projectGeneralizedVelocities(3);
359  reinitNeeded=true;
360  }
361  else if(gdMax>=0 and system->velocityDriftCompensationNeeded(gdMax)) {
362  system->projectGeneralizedVelocities(3);
363  reinitNeeded=true;
364  }
365  if(reinitNeeded) {
366  nrDriftCorr++;
367  BoostOdeintHelper::assign(zTemp, system->getState());
368  dos->initialize(zTemp, step.second, dos->current_time_step());
369  }
370  system->updateStopVectorParameters();
371  }
372  }
373  else {
374  // TODO: this code block (espezially the dos->initialize) should be avoided since initializting the integrator
375  // is expensive and not needed at macro sample points or updates of discrete states.
376  // For this the do_step call at the top must be restricted to reach at most tSamplePoint.
377  // However, this seem currently not possible with the boost odeint integrators.
378  // That's why we use this workaround here which has not the optimal performance.
379  dos->calc_state(tSamplePoint, zTemp);
380  curTimeAndState=tSamplePoint;
381  system->setTime(tSamplePoint);
382  BoostOdeintHelper::assign(system->getState(), zTemp);
383  dos->initialize(zTemp, tSamplePoint, dos->current_time_step());
384  system->updateStopVectorParameters();
385  }
386  }
387  }
388 
389  template<typename DOS>
390  void BoostOdeintDOS<DOS>::postIntegrate() {
391  msg(Info)<<std::endl;
392  msg(Info)<<"Integration statistics:"<<std::endl;
393  msg(Info)<<"nrSteps = "<<nrSteps<<std::endl;
394  msg(Info)<<"nrRHS = "<<nrRHS<<std::endl;
395  msg(Info)<<"nrJacs = "<<nrJacs<<std::endl;
396  msg(Info)<<"nrPlots = "<<nrPlots<<std::endl;
397  msg(Info)<<"nrSVs = "<<nrSVs<<std::endl;
398  msg(Info)<<"nrRoots = "<<nrRoots<<std::endl;
399  msg(Info)<<"nrDriftCorr = "<<nrDriftCorr<<std::endl;
400  }
401 
402  namespace {
403  // declaration
404  template<bool, typename Self>
405  struct InitializeUsingXMLControlled;
406 
407  // initializeUsingXML only used if isControlled == true
408  template<typename Self>
409  struct InitializeUsingXMLControlled<true, Self> {
410  static void call(Self self, xercesc::DOMElement* element) {
411  xercesc::DOMElement *e;
412 
413  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"absoluteToleranceScalar");
414  if(e) self->setAbsoluteTolerance(MBXMLUtils::E(e)->getText<double>());
415 
416  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"relativeToleranceScalar");
417  if(e) self->setRelativeTolerance(MBXMLUtils::E(e)->getText<double>());
418 
419  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"maximumStepSize");
420  if(e) self->setMaximumStepSize(MBXMLUtils::E(e)->getText<double>());
421  }
422  };
423 
424  // initializeUsingXML only used if isControlled == false
425  template<typename Self>
426  struct InitializeUsingXMLControlled<false, Self> {
427  static void call(Self self, xercesc::DOMElement* element) {
428  xercesc::DOMElement *e;
429 
430  // we are not checking in the XML schema for isControlled -> do it here at runtime
431 
432  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"absoluteToleranceScalar");
433  if(e) self->throwError("absoluteToleranceScalar element used for an fixed step-size stepper.");
434 
435  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"relativeToleranceScalar");
436  if(e) self->throwError("relativeToleranceScalar element used for an fixed step-size stepper.");
437 
438  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"maximumStepSize");
439  if(e) self->throwError("maximumStepSize element used for an fixed step-size stepper.");
440  }
441  };
442  }
443 
444  template<typename DOS>
445  void BoostOdeintDOS<DOS>::initializeUsingXML(xercesc::DOMElement *element) {
447  xercesc::DOMElement *e;
448 
449  InitializeUsingXMLControlled<isControlled, decltype(this)>::call(this, element);
450 
451  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"initialStepSize");
452  if(e) setInitialStepSize(MBXMLUtils::E(e)->getText<double>());
453  }
454 
455 }
456 
457 #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:445
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:223
Definition: boost_odeint_integrator.h:75
namespace MBSim
Definition: eigenanalyzer.cc:47