Directory: | ./ |
---|---|
File: | include/sot/core/integrator-abstract.hh |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 14 | 35 | 40.0% |
Branches: | 32 | 64 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Copyright 2010, | ||
3 | * François Bleibel, | ||
4 | * Olivier Stasse, | ||
5 | * | ||
6 | * CNRS/AIST | ||
7 | * | ||
8 | */ | ||
9 | |||
10 | #ifndef __SOT_INTEGRATOR_ABSTRACT_H__ | ||
11 | #define __SOT_INTEGRATOR_ABSTRACT_H__ | ||
12 | |||
13 | /* --------------------------------------------------------------------- */ | ||
14 | /* --- INCLUDE --------------------------------------------------------- */ | ||
15 | /* --------------------------------------------------------------------- */ | ||
16 | |||
17 | /* Matrix */ | ||
18 | #include <dynamic-graph/linear-algebra.h> | ||
19 | |||
20 | /* SOT */ | ||
21 | #include <dynamic-graph/all-signals.h> | ||
22 | #include <dynamic-graph/command-bind.h> | ||
23 | #include <dynamic-graph/entity.h> | ||
24 | #include <dynamic-graph/pool.h> | ||
25 | |||
26 | #include <sot/core/debug.hh> | ||
27 | #include <sot/core/flags.hh> | ||
28 | |||
29 | /* STD */ | ||
30 | #include <string> | ||
31 | |||
32 | /* --------------------------------------------------------------------- */ | ||
33 | /* --- CLASS ----------------------------------------------------------- */ | ||
34 | /* --------------------------------------------------------------------- */ | ||
35 | |||
36 | namespace dynamicgraph { | ||
37 | namespace sot { | ||
38 | |||
39 | /*! \brief integrates an ODE. If Y is the output and X the input, the | ||
40 | * following equation is integrated: | ||
41 | * a_p * d(p)Y / dt^p + .... + a_0 Y = b_m * d(m)X / dt^m + ... . b_0 X | ||
42 | * a_i are the coefficients of the denominator of the associated transfer | ||
43 | * function between X and Y, while the b_i are those of the numerator. | ||
44 | */ | ||
45 | template <class sigT, class coefT> | ||
46 | class IntegratorAbstract : public dynamicgraph::Entity { | ||
47 | public: | ||
48 | 2 | IntegratorAbstract(const std::string &name) | |
49 | : dynamicgraph::Entity(name), | ||
50 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | SIN(NULL, "sotIntegratorAbstract(" + name + ")::input(vector)::sin"), |
51 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | SOUT(boost::bind(&IntegratorAbstract<sigT, coefT>::integrate, this, _1, |
52 | _2), | ||
53 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | SIN, "sotIntegratorAbstract(" + name + ")::output(vector)::sout") { |
54 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | signalRegistration(SIN << SOUT); |
55 | |||
56 | using namespace dynamicgraph::command; | ||
57 | |||
58 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const std::string typeName = |
59 | Value::typeName(dynamicgraph::command::ValueHelper<coefT>::TypeID); | ||
60 | |||
61 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | addCommand( |
62 | "pushNumCoef", | ||
63 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
4 | makeCommandVoid1( |
64 | *this, &IntegratorAbstract::pushNumCoef, | ||
65 | docCommandVoid1("Push a new numerator coefficient", typeName))); | ||
66 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | addCommand( |
67 | "pushDenomCoef", | ||
68 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
4 | makeCommandVoid1( |
69 | *this, &IntegratorAbstract::pushDenomCoef, | ||
70 | docCommandVoid1("Push a new denominator coefficient", typeName))); | ||
71 | |||
72 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | addCommand( |
73 | "popNumCoef", | ||
74 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
4 | makeCommandVoid0(*this, &IntegratorAbstract::popNumCoef, |
75 | docCommandVoid0("Pop a new numerator coefficient"))); | ||
76 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | addCommand( |
77 | "popDenomCoef", | ||
78 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
4 | makeCommandVoid0(*this, &IntegratorAbstract::popDenomCoef, |
79 | docCommandVoid0("Pop a new denominator coefficient"))); | ||
80 | } | ||
81 | |||
82 | ✗ | virtual ~IntegratorAbstract() {} | |
83 | |||
84 | virtual sigT &integrate(sigT &res, int time) = 0; | ||
85 | |||
86 | public: | ||
87 | ✗ | void pushNumCoef(const coefT &numCoef) { numerator.push_back(numCoef); } | |
88 | ✗ | void pushDenomCoef(const coefT &denomCoef) { | |
89 | ✗ | denominator.push_back(denomCoef); | |
90 | } | ||
91 | ✗ | void popNumCoef() { numerator.pop_back(); } | |
92 | ✗ | void popDenomCoef() { denominator.pop_back(); } | |
93 | |||
94 | ✗ | const std::vector<coefT> &numCoeffs() const { return numerator; } | |
95 | ✗ | void numCoeffs(const std::vector<coefT> &coeffs) { numerator = coeffs; } | |
96 | |||
97 | ✗ | const std::vector<coefT> &denomCoeffs() const { return denominator; } | |
98 | ✗ | void denomCoeffs(const std::vector<coefT> &coeffs) { denominator = coeffs; } | |
99 | |||
100 | public: | ||
101 | dynamicgraph::SignalPtr<sigT, int> SIN; | ||
102 | |||
103 | dynamicgraph::SignalTimeDependent<sigT, int> SOUT; | ||
104 | |||
105 | ✗ | virtual void display(std::ostream &os) const { | |
106 | ✗ | os << this->getClassName() << ": " << getName() << '\n' << " "; | |
107 | ✗ | if (numerator.empty() || denominator.empty()) { | |
108 | ✗ | os << "ill-formed."; | |
109 | ✗ | return; | |
110 | } | ||
111 | ✗ | os << numerator[0]; | |
112 | ✗ | for (std::size_t i = 1; i < numerator.size(); ++i) | |
113 | ✗ | os << " + " << numerator[i] << " s^" << i; | |
114 | ✗ | os << "\n " << denominator[0]; | |
115 | ✗ | for (std::size_t i = 1; i < denominator.size(); ++i) | |
116 | ✗ | os << " + " << denominator[i] << " s^" << i; | |
117 | } | ||
118 | |||
119 | protected: | ||
120 | std::vector<coefT> numerator; | ||
121 | std::vector<coefT> denominator; | ||
122 | }; | ||
123 | |||
124 | } /* namespace sot */ | ||
125 | } /* namespace dynamicgraph */ | ||
126 | |||
127 | #endif | ||
128 |