explicit_integrators.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2020,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  *
22  * Authors: Christoph Rösmann
23  *********************************************************************/
24 
25 #ifndef SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_EXPLICIT_INTEGRATORS_H_
26 #define SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_EXPLICIT_INTEGRATORS_H_
27 
28 #include <corbo-communication/utilities.h>
29 #include <corbo-numerics/integrator_interface.h>
30 #include <memory>
31 
32 namespace corbo {
33 
47 class IntegratorExplicitEuler : public NumericalIntegratorExplicitInterface
48 {
49  public:
50  // Implements interface method
51  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorExplicitEuler>(); }
52 
53  // Implements interface method
54  int getConvergenceOrder() const override { return 1; }
55 
56  // Implements interface method
57  void solveIVP(const Eigen::VectorXd& x1, double dt, const std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)>& fun,
58  Eigen::Ref<Eigen::VectorXd> x2) override
59  {
60  fun(x1, x2);
61  x2 *= dt;
62  x2 += x1;
63  }
64 
65  // Implements interface method
66  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
67  Eigen::Ref<Eigen::VectorXd> x2) override
68  {
69  system.dynamics(x1, u1, x2);
70  x2 *= dt;
71  x2 += x1;
72  }
73 
74 #ifdef MESSAGE_SUPPORT
75  // Implements interface method
76  void toMessage(messages::ExplicitIntegrator& message) const override
77  {
78  NumericalIntegratorExplicitInterface::toMessage(message);
79  message.mutable_explicit_euler();
80  }
81 #endif
82 };
83 
84 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorExplicitEuler)
85 
86 
97 class IntegratorExplicitRungeKutta2 : public NumericalIntegratorExplicitInterface
98 {
99  public:
100  // Implements interface method
101  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorExplicitRungeKutta2>(); }
102 
103  // Implements interface method
104  int getConvergenceOrder() const override { return 2; }
105 
106  void initialize(int state_dim) override
107  {
108  _k1.resize(state_dim);
109  _k2.resize(state_dim);
110  }
111 
112  // Implements interface method
113  void solveIVP(const Eigen::VectorXd& x1, double dt, const std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)>& fun,
114  Eigen::Ref<Eigen::VectorXd> x2) override
115  {
116  if (x1.size() != _k1.size()) initialize(x1.size());
117 
118  fun(x1, _k1); // k1
119  _k1 *= dt;
120  fun(x1 + _k1, _k2); // k2
121  _k2 *= dt;
122 
123  x2 = x1 + (_k1 + _k2) / 2.0;
124  }
125 
126  // Implements interface method
127  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
128  Eigen::Ref<Eigen::VectorXd> x2) override
129  {
130  if (x1.size() != _k1.size()) initialize(x1.size());
131 
132  system.dynamics(x1, u1, _k1); // k1
133  _k1 *= dt;
134  system.dynamics(x1 + _k1, u1, _k2); // k2
135  _k2 *= dt;
136 
137  x2 = x1 + (_k1 + _k2) / 2.0;
138  }
139 
140 #ifdef MESSAGE_SUPPORT
141  // Implements interface method
142  void toMessage(messages::ExplicitIntegrator& message) const override
143  {
144  NumericalIntegratorExplicitInterface::toMessage(message);
145  message.mutable_runge_kutta_2();
146  }
147 #endif
148 
149  private:
150  Eigen::VectorXd _k1;
151  Eigen::VectorXd _k2;
152 };
153 
154 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorExplicitRungeKutta2)
155 
156 
167 class IntegratorExplicitRungeKutta3 : public NumericalIntegratorExplicitInterface
168 {
169  public:
170  // Implements interface method
171  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorExplicitRungeKutta3>(); }
172 
173  // Implements interface method
174  int getConvergenceOrder() const override { return 3; }
175 
176  void initialize(int state_dim) override
177  {
178  _k1.resize(state_dim);
179  _k2.resize(state_dim);
180  _k3.resize(state_dim);
181  }
182 
183  // Implements interface method
184  void solveIVP(const Eigen::VectorXd& x1, double dt, const std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)>& fun,
185  Eigen::Ref<Eigen::VectorXd> x2) override
186  {
187  if (x1.size() != _k1.size()) initialize(x1.size());
188 
189  fun(x1, _k1); // k1
190  _k1 *= dt;
191  fun(x1 + (_k1 / 2.0), _k2); // k2
192  _k2 *= dt;
193  fun(x1 - _k1 + 2.0 * _k2, _k3); // k3
194  _k3 *= dt;
195 
196  x2 = x1 + (_k1 + 4.0 * _k2 + _k3) / 6.0;
197  }
198 
199  // Implements interface method
200  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
201  Eigen::Ref<Eigen::VectorXd> x2) override
202  {
203  if (x1.size() != _k1.size()) initialize(x1.size());
204 
205  system.dynamics(x1, u1, _k1); // k1
206  _k1 *= dt;
207  system.dynamics(x1 + (_k1 / 2.0), u1, _k2); // k2
208  _k2 *= dt;
209  system.dynamics(x1 - _k1 + 2.0 * _k2, u1, _k3); // k3
210  _k3 *= dt;
211 
212  x2 = x1 + (_k1 + 4.0 * _k2 + _k3) / 6.0;
213  }
214 
215 #ifdef MESSAGE_SUPPORT
216  // Implements interface method
217  void toMessage(messages::ExplicitIntegrator& message) const override
218  {
219  NumericalIntegratorExplicitInterface::toMessage(message);
220  message.mutable_runge_kutta_3();
221  }
222 #endif
223 
224  private:
225  Eigen::VectorXd _k1;
226  Eigen::VectorXd _k2;
227  Eigen::VectorXd _k3;
228 };
229 
230 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorExplicitRungeKutta3)
231 
232 
244 class IntegratorExplicitRungeKutta4 : public NumericalIntegratorExplicitInterface
245 {
246  public:
247  // Implements interface method
248  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorExplicitRungeKutta4>(); }
249 
250  // Implements interface method
251  int getConvergenceOrder() const override { return 4; }
252 
253  void initialize(int state_dim) override
254  {
255  _k1.resize(state_dim);
256  _k2.resize(state_dim);
257  _k3.resize(state_dim);
258  _k4.resize(state_dim);
259  }
260 
261  // Implements interface method
262  void solveIVP(const Eigen::VectorXd& x1, double dt, const std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)>& fun,
263  Eigen::Ref<Eigen::VectorXd> x2) override
264  {
265  if (x1.size() != _k1.size()) initialize(x1.size());
266 
267  fun(x1, _k1); // k1
268  _k1 *= dt;
269  fun(x1 + _k1 / 2.0, _k2); // k2
270  _k2 *= dt;
271  fun(x1 + _k2 / 2.0, _k3); // k3
272  _k3 *= dt;
273  fun(x1 + _k3, _k4); // k4
274  _k4 *= dt;
275 
276  x2 = x1 + (_k1 + 2.0 * _k2 + 2.0 * _k3 + _k4) / 6.0; // TODO(roesmann) Is it possible to express k4 with x2 -> less temporaries
277  }
278 
279  // Implements interface method
280  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
281  Eigen::Ref<Eigen::VectorXd> x2) override
282  {
283  if (x1.size() != _k1.size()) initialize(x1.size());
284 
285  system.dynamics(x1, u1, _k1); // k1
286  _k1 *= dt;
287  system.dynamics(x1 + _k1 / 2.0, u1, _k2); // k2
288  _k2 *= dt;
289  system.dynamics(x1 + _k2 / 2.0, u1, _k3); // k3
290  _k3 *= dt;
291  system.dynamics(x1 + _k3, u1, _k4); // k4
292  _k4 *= dt;
293 
294  x2 = x1 + (_k1 + 2.0 * _k2 + 2.0 * _k3 + _k4) / 6.0; // TODO(roesmann) Is it possible to express k4 with x2 -> less temporaries
295  }
296 
297 #ifdef MESSAGE_SUPPORT
298  // Implements interface method
299  void toMessage(messages::ExplicitIntegrator& message) const override
300  {
301  NumericalIntegratorExplicitInterface::toMessage(message);
302  message.mutable_runge_kutta_4();
303  }
304 #endif
305 
306  private:
307  Eigen::VectorXd _k1;
308  Eigen::VectorXd _k2;
309  Eigen::VectorXd _k3;
310  Eigen::VectorXd _k4;
311 };
312 
313 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorExplicitRungeKutta4)
314 
315 
327 class IntegratorExplicitRungeKutta5 : public NumericalIntegratorExplicitInterface
328 {
329  public:
330  // Implements interface method
331  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorExplicitRungeKutta5>(); }
332 
333  // Implements interface method
334  int getConvergenceOrder() const override { return 5; }
335 
336  void initialize(int state_dim) override
337  {
338  _k1.resize(state_dim);
339  _k2.resize(state_dim);
340  _k3.resize(state_dim);
341  _k4.resize(state_dim);
342  _k5.resize(state_dim);
343  _k6.resize(state_dim);
344  }
345 
346  // Implements interface method
347  void solveIVP(const Eigen::VectorXd& x1, double dt, const std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)>& fun,
348  Eigen::Ref<Eigen::VectorXd> x2) override
349  {
350  if (x1.size() != _k1.size()) initialize(x1.size());
351 
352  fun(x1, _k1); // k1
353  _k1 *= dt;
354  fun(x1 + 4.0 * _k1 / 11.0, _k2); // k2
355  _k2 *= dt;
356  fun(x1 + (9.0 * _k1 + 11.0 * _k2) / 50.0, _k3); // k3
357  _k3 *= dt;
358  fun(x1 + (-11.0 * _k2 + 15.0 * _k3) / 4.0, _k4); // k4
359  _k4 *= dt;
360  fun(x1 + ((81.0 + 9.0 * std::sqrt(6.0)) * _k1 + (255.0 - 55.0 * std::sqrt(6.0)) * _k3 + (24.0 - 14.0 * std::sqrt(6.0)) * _k4) / 600.0,
361  _k5); // k5
362  _k5 *= dt;
363  fun(x1 + ((81.0 - 9.0 * std::sqrt(6.0)) * _k1 + (255.0 + 55.0 * std::sqrt(6.0)) * _k3 + (24.0 + 14.0 * std::sqrt(6.0)) * _k4) / 600.0,
364  _k6); // k6
365  _k6 *= dt;
366 
367  x2 = x1 + (4.0 * _k1 + (16.0 + std::sqrt(6.0)) * _k5 + (16.0 - std::sqrt(6.0)) * _k6) / 36.0;
368  }
369 
370  // Implements interface method
371  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
372  Eigen::Ref<Eigen::VectorXd> x2) override
373  {
374  if (x1.size() != _k1.size()) initialize(x1.size());
375 
376  system.dynamics(x1, u1, _k1); // k1
377  _k1 *= dt;
378  system.dynamics(x1 + 4.0 * _k1 / 11.0, u1, _k2); // k2
379  _k2 *= dt;
380  system.dynamics(x1 + (9.0 * _k1 + 11.0 * _k2) / 50.0, u1, _k3); // k3
381  _k3 *= dt;
382  system.dynamics(x1 + (-11.0 * _k2 + 15.0 * _k3) / 4.0, u1, _k4); // k4
383  _k4 *= dt;
384  system.dynamics(
385  x1 + ((81.0 + 9.0 * std::sqrt(6.0)) * _k1 + (255.0 - 55.0 * std::sqrt(6.0)) * _k3 + (24.0 - 14.0 * std::sqrt(6.0)) * _k4) / 600.0, u1,
386  _k5); // k5
387  _k5 *= dt;
388  system.dynamics(
389  x1 + ((81.0 - 9.0 * std::sqrt(6.0)) * _k1 + (255.0 + 55.0 * std::sqrt(6.0)) * _k3 + (24.0 + 14.0 * std::sqrt(6.0)) * _k4) / 600.0, u1,
390  _k6); // k6
391  _k6 *= dt;
392 
393  x2 = x1 + (4.0 * _k1 + (16.0 + std::sqrt(6.0)) * _k5 + (16.0 - std::sqrt(6.0)) * _k6) / 36.0;
394  }
395 
396 #ifdef MESSAGE_SUPPORT
397  // Implements interface method
398  void toMessage(messages::ExplicitIntegrator& message) const override
399  {
400  NumericalIntegratorExplicitInterface::toMessage(message);
401  message.mutable_runge_kutta_5();
402  }
403 #endif
404 
405  private:
406  Eigen::VectorXd _k1;
407  Eigen::VectorXd _k2;
408  Eigen::VectorXd _k3;
409  Eigen::VectorXd _k4;
410  Eigen::VectorXd _k5;
411  Eigen::VectorXd _k6;
412 };
413 
414 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorExplicitRungeKutta5)
415 
416 
429 class IntegratorExplicitRungeKutta6 : public NumericalIntegratorExplicitInterface
430 {
431  public:
432  // Implements interface method
433  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorExplicitRungeKutta6>(); }
434 
435  // Implements interface method
436  int getConvergenceOrder() const override { return 6; }
437 
438  void initialize(int state_dim) override
439  {
440  _k1.resize(state_dim);
441  _k2.resize(state_dim);
442  _k3.resize(state_dim);
443  _k4.resize(state_dim);
444  _k5.resize(state_dim);
445  _k6.resize(state_dim);
446  _k7.resize(state_dim);
447  _k8.resize(state_dim);
448  }
449 
450  // Implements interface method
451  void solveIVP(const Eigen::VectorXd& x1, double dt, const std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)>& fun,
452  Eigen::Ref<Eigen::VectorXd> x2) override
453  {
454  if (x1.size() != _k1.size()) initialize(x1.size());
455 
456  fun(x1, _k1); // k1
457  _k1 *= dt;
458  fun(x1 + 2.0 * _k1 / 33.0, _k2); // k2
459  _k2 *= dt;
460  fun(x1 + 4.0 * _k2 / 33.0, _k3); // k3
461  _k3 *= dt;
462  fun(x1 + (_k1 + 3.0 * _k3) / 22.0, _k4); // k4
463  _k4 *= dt;
464  fun(x1 + (43.0 * _k1 - 165.0 * _k3 + 144.0 * _k4) / 64.0, _k5); // k5
465  _k5 *= dt;
466  fun(x1 + (-4053483.0 * _k1 + 16334703.0 * _k3 - 12787632.0 * _k4 + 1057536.0 * _k5) / 826686.0, _k6); // k6
467  _k6 *= dt;
468  fun(x1 + (169364139.0 * _k1 - 663893307.0 * _k3 + 558275718.0 * _k4 - 29964480.0 * _k5 + 35395542.0 * _k6) / 80707214.0, _k7); // k7
469  _k7 *= dt;
470  fun(x1 + (-733.0 * _k1 + 3102.0 * _k3) / 176.0 - (335763.0 * _k4 / 23296.0) + (216.0 * _k5 / 77.0) - (4617.0 * _k6 / 2816.0) +
471  (7203.0 * _k7 / 9152.0),
472  _k8); // k8
473  _k8 *= dt;
474 
475  x2 = x1 + (336336.0 * _k1 + 1771561.0 * _k4 + 1916928.0 * _k5 + 597051.0 * _k6 + 1411788.0 * _k7 + 256256.0 * _k8) / 6289920.0;
476  }
477 
478  // Implements interface method
479  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
480  Eigen::Ref<Eigen::VectorXd> x2) override
481  {
482  if (x1.size() != _k1.size()) initialize(x1.size());
483 
484  system.dynamics(x1, u1, _k1); // k1
485  _k1 *= dt;
486  system.dynamics(x1 + 2.0 * _k1 / 33.0, u1, _k2); // k2
487  _k2 *= dt;
488  system.dynamics(x1 + 4.0 * _k2 / 33.0, u1, _k3); // k3
489  _k3 *= dt;
490  system.dynamics(x1 + (_k1 + 3.0 * _k3) / 22.0, u1, _k4); // k4
491  _k4 *= dt;
492  system.dynamics(x1 + (43.0 * _k1 - 165.0 * _k3 + 144.0 * _k4) / 64.0, u1, _k5); // k5
493  _k5 *= dt;
494  system.dynamics(x1 + (-4053483.0 * _k1 + 16334703.0 * _k3 - 12787632.0 * _k4 + 1057536.0 * _k5) / 826686.0, u1, _k6); // k6
495  _k6 *= dt;
496  system.dynamics(x1 + (169364139.0 * _k1 - 663893307.0 * _k3 + 558275718.0 * _k4 - 29964480.0 * _k5 + 35395542.0 * _k6) / 80707214.0, u1,
497  _k7); // k7
498  _k7 *= dt;
499  system.dynamics(x1 + (-733.0 * _k1 + 3102.0 * _k3) / 176.0 - (335763.0 * _k4 / 23296.0) + (216.0 * _k5 / 77.0) - (4617.0 * _k6 / 2816.0) +
500  (7203.0 * _k7 / 9152.0),
501  u1, _k8); // k8
502  _k8 *= dt;
503 
504  x2 = x1 + (336336.0 * _k1 + 1771561.0 * _k4 + 1916928.0 * _k5 + 597051.0 * _k6 + 1411788.0 * _k7 + 256256.0 * _k8) / 6289920.0;
505  }
506 
507 #ifdef MESSAGE_SUPPORT
508  // Implements interface method
509  void toMessage(messages::ExplicitIntegrator& message) const override
510  {
511  NumericalIntegratorExplicitInterface::toMessage(message);
512  message.mutable_runge_kutta_6();
513  }
514 #endif
515 
516  private:
517  Eigen::VectorXd _k1;
518  Eigen::VectorXd _k2;
519  Eigen::VectorXd _k3;
520  Eigen::VectorXd _k4;
521  Eigen::VectorXd _k5;
522  Eigen::VectorXd _k6;
523  Eigen::VectorXd _k7;
524  Eigen::VectorXd _k8;
525 };
526 
527 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorExplicitRungeKutta6)
528 
529 
541 class IntegratorExplicitRungeKutta7 : public NumericalIntegratorExplicitInterface
542 {
543  public:
544  // Implements interface method
545  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorExplicitRungeKutta7>(); }
546 
547  // Implements interface method
548  int getConvergenceOrder() const override { return 7; }
549 
550  void initialize(int state_dim) override
551  {
552  _k1.resize(state_dim);
553  _k2.resize(state_dim);
554  _k3.resize(state_dim);
555  _k4.resize(state_dim);
556  _k5.resize(state_dim);
557  _k6.resize(state_dim);
558  _k7.resize(state_dim);
559  _k8.resize(state_dim);
560  _k9.resize(state_dim);
561  _k10.resize(state_dim);
562  _k11.resize(state_dim);
563  }
564 
565  // Implements interface method
566  void solveIVP(const Eigen::VectorXd& x1, double dt, const std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)>& fun,
567  Eigen::Ref<Eigen::VectorXd> x2) override
568  {
569  if (x1.size() != _k1.size()) initialize(x1.size());
570 
571  fun(x1, _k1); // k1
572  _k1 *= dt;
573  fun(x1 + 2.0 * _k1 / 27.0, _k2); // k2
574  _k2 *= dt;
575  fun(x1 + (_k1 + 3.0 * _k2) / 36.0, _k3); // k3
576  _k3 *= dt;
577  fun(x1 + (_k1 + 3.0 * _k3) / 24.0, _k4); // k4
578  _k4 *= dt;
579  fun(x1 + (80.0 * _k1 - 300.0 * _k3 + 300.0 * _k4) / 192.0, _k5); // k5
580  _k5 *= dt;
581  fun(x1 + (_k1 + 5.0 * _k4 + 4.0 * _k5) / 20.0, _k6); // k6
582  _k6 *= dt;
583  fun(x1 + (-25.0 * _k1 + 125.0 * _k4 - 260.0 * _k5 + 250.0 * _k6) / 108.0, _k7); // k7
584  _k7 *= dt;
585  fun(x1 + (93.0 * _k1 + 244.0 * _k5 - 200.0 * _k6 + 13.0 * _k7) / 900.0, _k8); // k8
586  _k8 *= dt;
587  fun(x1 + (1080.0 * _k1 - 4770.0 * _k4 + 8448.0 * _k5 - 6420.0 * _k6 + 402.0 * _k7 + 1620.0 * _k8) / 540.0, _k9); // k9
588  _k9 *= dt;
589  fun(x1 + (-12285.0 * _k1 + 3105.0 * _k4 - 105408.0 * _k5 + 83970.0 * _k6 - 4617.0 * _k7 + 41310.0 * _k8 - 1215.0 * _k9) / 14580.0,
590  _k10); // k10
591  _k10 *= dt;
592  fun(x1 + (2383.0 * _k1 - 8525.0 * _k4 + 17984.0 * _k5 - 15050.0 * _k6 + 2133.0 * _k7 + 2250.0 * _k8 + 1125.0 * _k9 + 1800.0 * _k10) / 4100.0,
593  _k11); // k11
594  _k11 *= dt;
595 
596  x2 = x1 + (41.0 * _k1 + 272.0 * _k6 + 216.0 * _k7 + 216.0 * _k8 + 27.0 * _k9 + 27.0 * _k10 + 41.0 * _k11) / 840.0;
597  }
598 
599  // Implements interface method
600  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
601  Eigen::Ref<Eigen::VectorXd> x2) override
602  {
603  if (x1.size() != _k1.size()) initialize(x1.size());
604 
605  system.dynamics(x1, u1, _k1); // k1
606  _k1 *= dt;
607  system.dynamics(x1 + 2.0 * _k1 / 27.0, u1, _k2); // k2
608  _k2 *= dt;
609  system.dynamics(x1 + (_k1 + 3.0 * _k2) / 36.0, u1, _k3); // k3
610  _k3 *= dt;
611  system.dynamics(x1 + (_k1 + 3.0 * _k3) / 24.0, u1, _k4); // k4
612  _k4 *= dt;
613  system.dynamics(x1 + (80.0 * _k1 - 300.0 * _k3 + 300.0 * _k4) / 192.0, u1, _k5); // k5
614  _k5 *= dt;
615  system.dynamics(x1 + (_k1 + 5.0 * _k4 + 4.0 * _k5) / 20.0, u1, _k6); // k6
616  _k6 *= dt;
617  system.dynamics(x1 + (-25.0 * _k1 + 125.0 * _k4 - 260.0 * _k5 + 250.0 * _k6) / 108.0, u1, _k7); // k7
618  _k7 *= dt;
619  system.dynamics(x1 + (93.0 * _k1 + 244.0 * _k5 - 200.0 * _k6 + 13.0 * _k7) / 900.0, u1, _k8); // k8
620  _k8 *= dt;
621  system.dynamics(x1 + (12.0 * _k1 - 53.0 * _k4) / 6.0 + (1408.0 * _k5 - 1070.0 * _k6 + 67.0 * _k7 + 270.0 * _k8) / 90.0, u1, _k9); // k9
622  _k9 *= dt;
623  system.dynamics(x1 + (-12285.0 * _k1 + 3105.0 * _k4 - 105408.0 * _k5 + 83970.0 * _k6 - 4617.0 * _k7 + 41310.0 * _k8 - 1215.0 * _k9) / 14580.0,
624  u1, _k10); // k10
625  _k10 *= dt;
626  system.dynamics(
627  x1 + (2383.0 * _k1 - 8525.0 * _k4 + 17984.0 * _k5 - 15050.0 * _k6 + 2133.0 * _k7 + 2250.0 * _k8 + 1125.0 * _k9 + 1800.0 * _k10) / 4100.0,
628  u1, _k11); // k11
629  _k11 *= dt;
630 
631  x2 = x1 + (41.0 * _k1 + 272.0 * _k6 + 216.0 * _k7 + 216.0 * _k8 + 27.0 * _k9 + 27.0 * _k10 + 41.0 * _k11) / 840.0;
632  }
633 
634 #ifdef MESSAGE_SUPPORT
635  // Implements interface method
636  void toMessage(messages::ExplicitIntegrator& message) const override
637  {
638  NumericalIntegratorExplicitInterface::toMessage(message);
639  message.mutable_runge_kutta_7();
640  }
641 #endif
642 
643  private:
644  Eigen::VectorXd _k1;
645  Eigen::VectorXd _k2;
646  Eigen::VectorXd _k3;
647  Eigen::VectorXd _k4;
648  Eigen::VectorXd _k5;
649  Eigen::VectorXd _k6;
650  Eigen::VectorXd _k7;
651  Eigen::VectorXd _k8;
652  Eigen::VectorXd _k9;
653  Eigen::VectorXd _k10;
654  Eigen::VectorXd _k11;
655 };
656 
657 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorExplicitRungeKutta7)
658 
659 
672 class IntegratorAdaptiveStepSize : public NumericalIntegratorExplicitInterface
673 {
674  public:
675  // Implements interface method
676  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorAdaptiveStepSize>(); }
677 
678  // Implements interface method
679  int getConvergenceOrder() const override { return _p2; }
680 
681  void initialize(int state_dim) override
682  {
683  _integrator1->initialize(state_dim);
684  _integrator2->initialize(state_dim);
685  }
686 
687  // Implements interface method
688  void solveIVP(const Eigen::VectorXd& x1, double dt, const std::function<void(const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)>& fun,
689  Eigen::Ref<Eigen::VectorXd> x2) override
690  {
691  // start-values:
692  _tau = 0.0;
693  _tau2 = 0.0;
694  _h_new = dt;
695 
696  // initialization
697  _help2 = x1;
698  x2 = x1;
699  _help = _help2;
700 
701  // start-loop:
702  while (std::abs(_tau - dt) > std::numeric_limits<double>::epsilon())
703  {
704  _h = _h_new;
705  if ((_tau + _h) > dt)
706  {
707  _h = dt - _tau;
708  }
709 
710  while (true)
711  {
712  _tau2 = _tau + _h;
713 
714  // solve the integration-problem
715  _integrator1->solveIVP(_help2, _h, fun, _help);
716  _integrator2->solveIVP(_help2, _h, fun, x2);
717 
718  // evaluating the error in the in the discrete l²-norm
719  _epsilon_hat = 0.0;
720  _help = _help - x2;
721  for (int k = 0; k < _help.size(); k++)
722  {
723  _epsilon_hat = _epsilon_hat + pow(_help(k), 2.0);
724  }
725  _epsilon_hat = std::pow(_epsilon_hat, 0.5);
726 
727  // evaluation of the new step-size
728  _h_new = pow(0.9 * (_tol / _epsilon_hat), (1.0 / (double(_p1) + 1.0))) * _h;
729  if (_epsilon_hat > _tol)
730  {
731  _h = _h_new;
732  }
733  else
734  {
735  break;
736  }
737  }
738 
739  // update parameters
740  _tau = _tau2;
741  _help2 = x2;
742  }
743  }
744 
745  // Implements interface method
746  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
747  Eigen::Ref<Eigen::VectorXd> x2) override
748  {
749  // start-values:
750  _tau = 0.0;
751  _tau2 = 0.0;
752  _h_new = dt;
753 
754  // initialization
755  _help2 = x1;
756  x2 = x1;
757  _help = _help2;
758 
759  // start-loop:
760  while (std::abs(_tau - dt) > std::numeric_limits<double>::epsilon())
761  {
762  _h = _h_new;
763  if ((_tau + _h) > dt)
764  {
765  _h = dt - _tau;
766  }
767 
768  while (true)
769  {
770  _tau2 = _tau + _h;
771 
772  // solve the integration-problem
773  _integrator1->solveIVP(_help2, u1, _h, system, _help);
774  _integrator2->solveIVP(_help2, u1, _h, system, x2);
775 
776  // evaluating the error in the in the discrete l²-norm
777  _epsilon_hat = 0.0;
778  _help = _help - x2;
779  for (int k = 0; k < _help.size(); k++)
780  {
781  _epsilon_hat = _epsilon_hat + pow(_help(k), 2.0);
782  }
783  _epsilon_hat = pow(_epsilon_hat, 0.5);
784 
785  // evaluation of the new step-size
786  _h_new = std::pow(0.9 * (_tol / _epsilon_hat), (1.0 / (double(_p1) + 1.0))) * _h;
787  if (_epsilon_hat > _tol)
788  {
789  _h = _h_new;
790  }
791  else
792  {
793  break;
794  }
795  }
796 
797  // update parameters
798  _tau = _tau2;
799  _help2 = x2;
800  }
801  }
802 
803 #ifdef MESSAGE_SUPPORT
804  void toMessage(messages::IntegratorAdaptiveStepSize& message) const
805  {
806  message.set_tol(_tol);
807  if (_integrator1) _integrator1->toMessage(*message.mutable_integrator1());
808  if (_integrator2) _integrator2->toMessage(*message.mutable_integrator2());
809  }
810  void fromMessage(const messages::IntegratorAdaptiveStepSize& message, std::stringstream* issues = nullptr)
811  {
812  _tol = message.tol();
813  // create integrator1
814  std::string type;
815  util::get_oneof_field_type(message.integrator1(), "explicit_integrator", type, false);
816  NumericalIntegratorExplicitInterface::Ptr int1 = create_from_factory<NumericalIntegratorExplicitInterface>(type);
817  // import parameters
818  if (int1)
819  {
820  int1->fromMessage(message.integrator1(), issues);
821  _integrator1 = int1;
822  _p1 = int1->getConvergenceOrder();
823  }
824  else
825  {
826  if (issues) *issues << "IntegratorAdaptiveStepSize: unknown integrator1 specified.\n";
827  return;
828  }
829  // create integrator2
830  type = "";
831  util::get_oneof_field_type(message.integrator2(), "explicit_integrator", type, false);
832  NumericalIntegratorExplicitInterface::Ptr int2 = create_from_factory<NumericalIntegratorExplicitInterface>(type);
833  // import parameters
834  if (int2)
835  {
836  int2->fromMessage(message.integrator2(), issues);
837  _integrator2 = int2;
838  _p2 = int2->getConvergenceOrder();
839  }
840  else
841  {
842  if (issues) *issues << "IntegratorAdaptiveStepSize: unknown integrator2 specified.\n";
843  return;
844  }
845  // error-message if _p1 >= _p2
846  if (_p1 >= _p2 && issues)
847  *issues << "The chosen methods are not applicable! The order of the first method has to be lower than the order of the second method!\n";
848  }
849 
850  void toMessage(messages::ExplicitIntegrator& message) const override
851  {
852  NumericalIntegratorExplicitInterface::toMessage(message);
853  toMessage(*message.mutable_adaptive_step_size());
854  }
855  void fromMessage(const messages::DynamicsEval& message, std::stringstream* issues = nullptr) override
856  {
857  NumericalIntegratorExplicitInterface::fromMessage(message, issues);
858  fromMessage(message.integrator().adaptive_step_size(), issues);
859  }
860 #endif
861  protected:
862  double _tol;
863  NumericalIntegratorExplicitInterface::Ptr _integrator1 = std::make_shared<IntegratorExplicitRungeKutta2>();
864  NumericalIntegratorExplicitInterface::Ptr _integrator2 = std::make_shared<IntegratorExplicitRungeKutta7>();
865 
866  int _p1 = 0;
867  int _p2 = 0;
868 
869  private:
870  double _epsilon_hat;
871  double _tau;
872  double _tau2;
873  double _h;
874  double _h_new;
875  Eigen::VectorXd _help;
876  Eigen::VectorXd _help2;
877 };
878 
879 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorAdaptiveStepSize)
880 
881 
890 class IntegratorMultiStageFixedStep : public NumericalIntegratorExplicitInterface
891 {
892  public:
893  // Implements interface method
894  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorMultiStageFixedStep>(); }
895 
896  // Implements interface method
897  int getConvergenceOrder() const override { return _integrator->getConvergenceOrder(); }
898 
899  // Implements interface method
900  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
901  Eigen::Ref<Eigen::VectorXd> x2) override
902  {
903  assert(_integrator);
904  if (dt <= _inner_integration_dt || _inner_integration_dt <= 0)
905  {
906  _integrator->solveIVP(x1, u1, dt, system, x2);
907  return;
908  }
909 
910  int n = dt / _inner_integration_dt;
911  double dt_remainder = std::fmod(dt, _inner_integration_dt);
912 
913  _x_sub.resize(x1.size());
914  _integrator->solveIVP(x1, u1, _inner_integration_dt, system, _x_sub);
915  for (int i = 1; i < n; ++i)
916  {
917  _integrator->solveIVP(_x_sub, u1, _inner_integration_dt, system, x2);
918  _x_sub = x2;
919  }
920  if (dt_remainder > 0) _integrator->solveIVP(_x_sub, u1, dt_remainder, system, x2);
921  }
922 
923  // Implements interface method
924  void solveIVP(const Eigen::VectorXd& x1, double dt, const UnaryFunction& fun, Eigen::Ref<Eigen::VectorXd> x2) override
925  {
926  assert(_integrator);
927  if (dt <= _inner_integration_dt || _inner_integration_dt <= 0)
928  {
929  _integrator->solveIVP(x1, dt, fun, x2);
930  return;
931  }
932 
933  int n = dt / _inner_integration_dt;
934  double dt_remainder = std::fmod(dt, _inner_integration_dt);
935 
936  _x_sub.resize(x1.size());
937  _integrator->solveIVP(x1, _inner_integration_dt, fun, _x_sub);
938  for (int i = 1; i < n; ++i)
939  {
940  _integrator->solveIVP(_x_sub, _inner_integration_dt, fun, x2);
941  _x_sub = x2;
942  }
943  if (dt_remainder > 1e-8)
944  _integrator->solveIVP(_x_sub, dt_remainder, fun, x2); // TODO(roesmann): maybe use std::numerical_limits::epsilon or round_error
945  }
946 
947  void setSubIntegrator(NumericalIntegratorExplicitInterface::Ptr integrator) { _integrator = integrator; }
948 
949 #ifdef MESSAGE_SUPPORT
950  void fromMessage(const messages::IntegratorMultiStageFixedStep& message, std::stringstream* issues = nullptr)
951  {
952  _inner_integration_dt = message.inner_integration_dt();
953 
954  // create integrator
955  std::string type;
956  util::get_oneof_field_type(message.integrator(), "explicit_integrator", type, false);
957  NumericalIntegratorExplicitInterface::Ptr integrator = create_from_factory<NumericalIntegratorExplicitInterface>(type);
958  // import parameters
959  if (integrator)
960  {
961  integrator->fromMessage(message.integrator(), issues);
962  _integrator = integrator;
963  }
964  else
965  {
966  if (issues) *issues << "IntegratorMultiStageFixedStep: unknown integrator specified.\n";
967  return;
968  }
969  }
970 
971  void toMessage(messages::IntegratorMultiStageFixedStep& message) const
972  {
973  message.set_inner_integration_dt(_inner_integration_dt);
974 
975  if (_integrator) _integrator->toMessage(*message.mutable_integrator());
976  }
977 
978  // Implements interface method
979  void fromMessage(const messages::ExplicitIntegrator& message, std::stringstream* issues = nullptr) override
980  {
981  NumericalIntegratorExplicitInterface::fromMessage(message);
982  fromMessage(message.multi_stage_fixed_step());
983  }
984 
985  // Implements interface method
986  void toMessage(messages::ExplicitIntegrator& message) const override
987  {
988  NumericalIntegratorExplicitInterface::toMessage(message);
989  toMessage(*message.mutable_multi_stage_fixed_step());
990  }
991 #endif
992 
993  private:
994  NumericalIntegratorExplicitInterface::Ptr _integrator;
995  double _inner_integration_dt = 0.1;
996 
997  Eigen::VectorXd _x_sub;
998 };
999 
1000 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorMultiStageFixedStep)
1001 
1002 
1011 class IntegratorMultiStageScaled : public NumericalIntegratorExplicitInterface
1012 {
1013  public:
1014  // Implements interface method
1015  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<IntegratorMultiStageScaled>(); }
1016 
1017  // Implements interface method
1018  int getConvergenceOrder() const override { return _integrator->getConvergenceOrder(); }
1019 
1020  // Implements interface method
1021  void solveIVP(const StateVector& x1, const InputVector& u1, double dt, const SystemDynamicsInterface& system,
1022  Eigen::Ref<Eigen::VectorXd> x2) override
1023  {
1024  assert(_n > 0);
1025 
1026  double inner_dt = dt / (double)_n;
1027 
1028  _x_sub.resize(x1.size());
1029  _integrator->solveIVP(x1, u1, inner_dt, system, _x_sub);
1030  for (int i = 1; i < _n; ++i)
1031  {
1032  _integrator->solveIVP(_x_sub, u1, inner_dt, system, x2);
1033  _x_sub = x2;
1034  }
1035  }
1036 
1037  // Implements interface method
1038  void solveIVP(const Eigen::VectorXd& x1, double dt, const UnaryFunction& fun, Eigen::Ref<Eigen::VectorXd> x2) override
1039  {
1040  assert(_n > 0);
1041 
1042  double inner_dt = dt / (double)_n;
1043 
1044  _x_sub.resize(x1.size());
1045  _integrator->solveIVP(x1, inner_dt, fun, _x_sub);
1046  for (int i = 1; i < _n; ++i)
1047  {
1048  _integrator->solveIVP(_x_sub, inner_dt, fun, x2);
1049  _x_sub = x2;
1050  }
1051  }
1052 
1053  void setSubIntegrator(NumericalIntegratorExplicitInterface::Ptr integrator) { _integrator = integrator; }
1054  void setN(int n) { _n = n; }
1055 
1056 #ifdef MESSAGE_SUPPORT
1057  void fromMessage(const messages::IntegratorMultiStageScaled& message, std::stringstream* issues = nullptr)
1058  {
1059  _n = message.n();
1060 
1061  // create integrator
1062  std::string type;
1063  util::get_oneof_field_type(message.integrator(), "explicit_integrator", type, false);
1064  NumericalIntegratorExplicitInterface::Ptr integrator = create_from_factory<NumericalIntegratorExplicitInterface>(type);
1065  // import parameters
1066  if (integrator)
1067  {
1068  integrator->fromMessage(message.integrator(), issues);
1069  _integrator = integrator;
1070  }
1071  else
1072  {
1073  if (issues) *issues << "IntegratorMultiStageScaled: unknown integrator specified.\n";
1074  return;
1075  }
1076  }
1077 
1078  void toMessage(messages::IntegratorMultiStageScaled& message) const
1079  {
1080  message.set_n(_n);
1081 
1082  if (_integrator) _integrator->toMessage(*message.mutable_integrator());
1083  }
1084 
1085  // Implements interface method
1086  void fromMessage(const messages::ExplicitIntegrator& message, std::stringstream* issues = nullptr) override
1087  {
1088  NumericalIntegratorExplicitInterface::fromMessage(message);
1089  fromMessage(message.multi_stage_scaled());
1090  }
1091 
1092  // Implements interface method
1093  void toMessage(messages::ExplicitIntegrator& message) const override
1094  {
1095  NumericalIntegratorExplicitInterface::toMessage(message);
1096  toMessage(*message.mutable_multi_stage_scaled());
1097  }
1098 #endif
1099 
1100  private:
1101  NumericalIntegratorExplicitInterface::Ptr _integrator;
1102  int _n = 10;
1103 
1104  Eigen::VectorXd _x_sub;
1105 };
1106 
1107 FACTORY_REGISTER_EXPLICIT_INTEGRATOR(IntegratorMultiStageScaled)
1108 
1109 } // namespace corbo
1110 
1111 #endif // SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_EXPLICIT_INTEGRATORS_H_
corbo::SystemDynamicsInterface::dynamics
virtual void dynamics(const Eigen::Ref< const StateVector > &x, const Eigen::Ref< const ControlVector > &u, Eigen::Ref< StateVector > f) const =0
Evaluate the system dynamics equation.
sqrt
const EIGEN_DEVICE_FUNC SqrtReturnType sqrt() const
Definition: ArrayCwiseUnaryOps.h:152
corbo::IntegratorExplicitRungeKutta2::solveIVP
void solveIVP(const Eigen::VectorXd &x1, double dt, const std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> &fun, Eigen::Ref< Eigen::VectorXd > x2) override
Definition: explicit_integrators.h:135
corbo::IntegratorExplicitRungeKutta3::_k3
Eigen::VectorXd _k3
Definition: explicit_integrators.h:249
corbo::IntegratorExplicitRungeKutta4::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: explicit_integrators.h:270
corbo::IntegratorExplicitRungeKutta2::_k2
Eigen::VectorXd _k2
Definition: explicit_integrators.h:173
corbo::SystemDynamicsInterface
Interface class for system dynamic equations.
Definition: system_dynamics_interface.h:88
corbo::IntegratorExplicitRungeKutta6
6th Order Runge-Kutta Integrator (explicit)
Definition: explicit_integrators.h:451
corbo::IntegratorExplicitRungeKutta5::_k3
Eigen::VectorXd _k3
Definition: explicit_integrators.h:430
corbo::IntegratorMultiStageScaled::_x_sub
Eigen::VectorXd _x_sub
Definition: explicit_integrators.h:1126
Eigen::half_impl::pow
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half pow(const half &a, const half &b)
Definition: Half.h:477
integrator_interface.h
corbo::IntegratorExplicitRungeKutta4::_k3
Eigen::VectorXd _k3
Definition: explicit_integrators.h:331
corbo
Definition: communication/include/corbo-communication/utilities.h:37
corbo::NumericalIntegratorExplicitInterface::Ptr
std::shared_ptr< NumericalIntegratorExplicitInterface > Ptr
Definition: integrator_interface.h:205
corbo::IntegratorExplicitRungeKutta5::solveIVP
void solveIVP(const Eigen::VectorXd &x1, double dt, const std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> &fun, Eigen::Ref< Eigen::VectorXd > x2) override
Definition: explicit_integrators.h:369
corbo::IntegratorExplicitRungeKutta2::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: explicit_integrators.h:123
corbo::IntegratorExplicitRungeKutta3::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: explicit_integrators.h:193
corbo::IntegratorExplicitRungeKutta5::_k5
Eigen::VectorXd _k5
Definition: explicit_integrators.h:432
corbo::IntegratorExplicitRungeKutta5::_k4
Eigen::VectorXd _k4
Definition: explicit_integrators.h:431
corbo::IntegratorExplicitRungeKutta2::_k1
Eigen::VectorXd _k1
Definition: explicit_integrators.h:172
corbo::IntegratorMultiStageFixedStep::setSubIntegrator
void setSubIntegrator(NumericalIntegratorExplicitInterface::Ptr integrator)
Definition: explicit_integrators.h:969
corbo::IntegratorExplicitEuler::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: explicit_integrators.h:95
corbo::IntegratorExplicitRungeKutta5::_k2
Eigen::VectorXd _k2
Definition: explicit_integrators.h:429
corbo::IntegratorExplicitEuler::getConvergenceOrder
int getConvergenceOrder() const override
Return the convergence order.
Definition: explicit_integrators.h:98
corbo::IntegratorExplicitRungeKutta3::_k2
Eigen::VectorXd _k2
Definition: explicit_integrators.h:248
corbo::NumericalIntegratorExplicitInterface
‍**
Definition: integrator_interface.h:180
abs
EIGEN_DEVICE_FUNC const EIGEN_STRONG_INLINE AbsReturnType abs() const
Definition: ArrayCwiseUnaryOps.h:43
corbo::IntegratorExplicitRungeKutta3::initialize
void initialize(int state_dim) override
Allocate memory for a given state dimension.
Definition: explicit_integrators.h:198
corbo::IntegratorExplicitRungeKutta4::solveIVP
void solveIVP(const Eigen::VectorXd &x1, double dt, const std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> &fun, Eigen::Ref< Eigen::VectorXd > x2) override
Definition: explicit_integrators.h:284
corbo::IntegratorExplicitRungeKutta5::initialize
void initialize(int state_dim) override
Allocate memory for a given state dimension.
Definition: explicit_integrators.h:358
corbo::IntegratorMultiStageFixedStep::getConvergenceOrder
int getConvergenceOrder() const override
Return the convergence order.
Definition: explicit_integrators.h:919
corbo::IntegratorExplicitRungeKutta2::initialize
void initialize(int state_dim) override
Allocate memory for a given state dimension.
Definition: explicit_integrators.h:128
corbo::IntegratorMultiStageFixedStep::_integrator
NumericalIntegratorExplicitInterface::Ptr _integrator
Definition: explicit_integrators.h:1016
corbo::IntegratorMultiStageScaled::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: explicit_integrators.h:1037
corbo::IntegratorMultiStageFixedStep::_inner_integration_dt
double _inner_integration_dt
Definition: explicit_integrators.h:1017
Eigen::numext::fmod
EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T fmod(const T &a, const T &b)
Definition: Eigen/src/Core/MathFunctions.h:1244
corbo::IntegratorAdaptiveStepSize
Adaptive-Step-Size-Control.
Definition: explicit_integrators.h:694
corbo::IntegratorExplicitRungeKutta4::getConvergenceOrder
int getConvergenceOrder() const override
Return the convergence order.
Definition: explicit_integrators.h:273
corbo::IntegratorExplicitRungeKutta5::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: explicit_integrators.h:353
corbo::IntegratorExplicitRungeKutta4::initialize
void initialize(int state_dim) override
Allocate memory for a given state dimension.
Definition: explicit_integrators.h:275
corbo::IntegratorMultiStageFixedStep::_x_sub
Eigen::VectorXd _x_sub
Definition: explicit_integrators.h:1019
corbo::DynamicsEvalInterface::StateVector
Eigen::VectorXd StateVector
Definition: dynamics_eval_interface.h:104
corbo::IntegratorMultiStageScaled::getConvergenceOrder
int getConvergenceOrder() const override
Return the convergence order.
Definition: explicit_integrators.h:1040
corbo::IntegratorExplicitRungeKutta5::getConvergenceOrder
int getConvergenceOrder() const override
Return the convergence order.
Definition: explicit_integrators.h:356
corbo::IntegratorExplicitRungeKutta4
4th Order Runge-Kutta Integrator (explicit)
Definition: explicit_integrators.h:266
corbo::IntegratorExplicitRungeKutta2
2th Order Runge-Kutta Integrator (explicit)
Definition: explicit_integrators.h:119
corbo::IntegratorMultiStageFixedStep::solveIVP
void solveIVP(const StateVector &x1, const InputVector &u1, double dt, const SystemDynamicsInterface &system, Eigen::Ref< Eigen::VectorXd > x2) override
Solution of the initial value problem.
Definition: explicit_integrators.h:922
utilities.h
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
corbo::IntegratorExplicitRungeKutta5::_k1
Eigen::VectorXd _k1
Definition: explicit_integrators.h:428
corbo::IntegratorExplicitRungeKutta4::_k2
Eigen::VectorXd _k2
Definition: explicit_integrators.h:330
corbo::IntegratorExplicitRungeKutta4::_k1
Eigen::VectorXd _k1
Definition: explicit_integrators.h:329
corbo::IntegratorExplicitRungeKutta3::getConvergenceOrder
int getConvergenceOrder() const override
Return the convergence order.
Definition: explicit_integrators.h:196
corbo::IntegratorExplicitRungeKutta7
7th Order Runge-Kutta Integrator (explicit)
Definition: explicit_integrators.h:563
corbo::IntegratorExplicitRungeKutta5::_k6
Eigen::VectorXd _k6
Definition: explicit_integrators.h:433
corbo::IntegratorExplicitRungeKutta4::_k4
Eigen::VectorXd _k4
Definition: explicit_integrators.h:332
corbo::IntegratorExplicitEuler::solveIVP
void solveIVP(const Eigen::VectorXd &x1, double dt, const std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> &fun, Eigen::Ref< Eigen::VectorXd > x2) override
Definition: explicit_integrators.h:101
corbo::IntegratorMultiStageScaled::solveIVP
void solveIVP(const StateVector &x1, const InputVector &u1, double dt, const SystemDynamicsInterface &system, Eigen::Ref< Eigen::VectorXd > x2) override
Solution of the initial value problem.
Definition: explicit_integrators.h:1043
corbo::IntegratorExplicitRungeKutta3::solveIVP
void solveIVP(const Eigen::VectorXd &x1, double dt, const std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> &fun, Eigen::Ref< Eigen::VectorXd > x2) override
Definition: explicit_integrators.h:206
corbo::IntegratorMultiStageScaled::_integrator
NumericalIntegratorExplicitInterface::Ptr _integrator
Definition: explicit_integrators.h:1123
corbo::IntegratorMultiStageScaled::setSubIntegrator
void setSubIntegrator(NumericalIntegratorExplicitInterface::Ptr integrator)
Definition: explicit_integrators.h:1075
corbo::IntegratorExplicitRungeKutta3::_k1
Eigen::VectorXd _k1
Definition: explicit_integrators.h:247
corbo::IntegratorMultiStageScaled::_n
int _n
Definition: explicit_integrators.h:1124
FACTORY_REGISTER_EXPLICIT_INTEGRATOR
#define FACTORY_REGISTER_EXPLICIT_INTEGRATOR(type)
Definition: integrator_interface.h:318
corbo::DynamicsEvalInterface::InputVector
Eigen::VectorXd InputVector
Definition: dynamics_eval_interface.h:105
n
PlainMatrixType mat * n
Definition: eigenvalues.cpp:41
corbo::IntegratorExplicitRungeKutta3
3th Order Runge-Kutta Integrator (explicit)
Definition: explicit_integrators.h:189
corbo::NumericalIntegratorExplicitInterface::UnaryFunction
const std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> UnaryFunction
Definition: integrator_interface.h:208
corbo::IntegratorExplicitRungeKutta2::getConvergenceOrder
int getConvergenceOrder() const override
Return the convergence order.
Definition: explicit_integrators.h:126
corbo::DynamicsEvalInterface::Ptr
std::shared_ptr< DynamicsEvalInterface > Ptr
Definition: dynamics_eval_interface.h:101
corbo::IntegratorMultiStageFixedStep::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: explicit_integrators.h:916
corbo::IntegratorMultiStageScaled::setN
void setN(int n)
Definition: explicit_integrators.h:1076

control_box_rst
Author(s): Christoph Rösmann
autogenerated on Wed Mar 2 2022 00:05:45