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 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>
75 class BoostOdeintSystem;
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>
103 class BoostOdeintDOS : public RootFindingIntegrator {
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>
226 void BoostOdeintDOS<DOS>::integrate() {
227 preIntegrate();
228 subIntegrate(tEnd);
229 postIntegrate();
230 }
231
232 template<typename DOS>
233 void BoostOdeintDOS<DOS>::preIntegrate() {
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) {
498 RootFindingIntegrator::initializeUsingXML(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
MBSim::BoostOdeintDOS::setMaximumStepSize
void setMaximumStepSize(double dtMax_)
Set maximum step size.
Definition: boost_odeint_integrator.h:132
MBSim::RootFindingIntegrator
Integrator with root-finding.
Definition: root_finding_integrator.h:32
MBSim::BoostOdeintDOS::setRelativeTolerance
void setRelativeTolerance(double rTol_)
Set relative tolerance.
Definition: boost_odeint_integrator.h:128
MBSim::BoostOdeintHelper::ImplicitSystemTag
Definition: boost_odeint_integrator.h:69
MBSim::BoostOdeintHelper::ExplicitSystemTag
Definition: boost_odeint_integrator.h:67
MBSim::BoostOdeintDOS::setInitialStepSize
void setInitialStepSize(double dt0_)
Set initial step size.
Definition: boost_odeint_integrator.h:120
MBSim::BoostOdeintDOS
Definition: boost_odeint_integrator.h:103
MBSim::BoostOdeintDOS::initializeUsingXML
void initializeUsingXML(xercesc::DOMElement *element) override
initialize integrator
Definition: boost_odeint_integrator.h:497
MBSim::RootFindingIntegrator::initializeUsingXML
virtual void initializeUsingXML(xercesc::DOMElement *element)
initialize integrator
Definition: root_finding_integrator.cc:44
MBSim::BoostOdeintDOS::setAbsoluteTolerance
void setAbsoluteTolerance(double aTol_)
Set absolute tolerance.
Definition: boost_odeint_integrator.h:124
MBSim::BoostOdeintDOS::integrate
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
MBSim::BoostOdeintHelper::BoostOdeintSystem
Definition: boost_odeint_integrator.h:75
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30