Directory: | ./ |
---|---|
File: | src/filters/filter-differentiator.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 38 | 56 | 67.9% |
Branches: | 95 | 234 | 40.6% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Copyright 2017-, Rohan Budhiraja LAAS-CNRS | ||
3 | * | ||
4 | * This file is part of sot-torque-control. | ||
5 | * sot-torque-control is free software: you can redistribute it and/or | ||
6 | * modify it under the terms of the GNU Lesser General Public License | ||
7 | * as published by the Free Software Foundation, either version 3 of | ||
8 | * the License, or (at your option) any later version. | ||
9 | * sot-torque-control is distributed in the hope that it will be | ||
10 | * useful, but WITHOUT ANY WARRANTY; without even the implied warranty | ||
11 | * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
12 | * GNU Lesser General Public License for more details. You should | ||
13 | * have received a copy of the GNU Lesser General Public License along | ||
14 | * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>. | ||
15 | */ | ||
16 | |||
17 | #define LOGFILE "/tmp/fd_log.dat" | ||
18 | |||
19 | #define LOG(x) \ | ||
20 | { \ | ||
21 | std::ofstream LogFile; \ | ||
22 | LogFile.open(LOGFILE, std::ofstream::app); \ | ||
23 | LogFile << x << std::endl; \ | ||
24 | LogFile.close(); \ | ||
25 | } | ||
26 | |||
27 | #include <dynamic-graph/all-commands.h> | ||
28 | #include <dynamic-graph/factory.h> | ||
29 | |||
30 | #include <sot/core/debug.hh> | ||
31 | #include <sot/core/filter-differentiator.hh> | ||
32 | //#include <sot/torque_control/motor-model.hh> | ||
33 | #include <Eigen/Dense> | ||
34 | |||
35 | namespace dynamicgraph { | ||
36 | namespace sot { | ||
37 | |||
38 | #define ALL_INPUT_SIGNALS m_xSIN | ||
39 | |||
40 | #define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT | ||
41 | |||
42 | namespace dynamicgraph = ::dynamicgraph; | ||
43 | using namespace dynamicgraph; | ||
44 | using namespace dynamicgraph::command; | ||
45 | using namespace Eigen; | ||
46 | |||
47 | /// Define EntityClassName here rather than in the header file | ||
48 | /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. | ||
49 | typedef FilterDifferentiator EntityClassName; | ||
50 | |||
51 | /* --- DG FACTORY --------------------------------------------------- */ | ||
52 | ✗ | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FilterDifferentiator, | |
53 | "FilterDifferentiator"); | ||
54 | |||
55 | /* --- CONSTRUCTION ------------------------------------------------- */ | ||
56 | /* --- CONSTRUCTION ------------------------------------------------- */ | ||
57 | /* --- CONSTRUCTION ------------------------------------------------- */ | ||
58 | 1 | FilterDifferentiator::FilterDifferentiator(const std::string &name) | |
59 | : Entity(name), | ||
60 |
7/14✓ 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.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
|
1 | CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector), |
61 |
10/20✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
|
1 | CONSTRUCT_SIGNAL_OUT(x_filtered, dynamicgraph::Vector, m_x_dx_ddxSINNER), |
62 |
10/20✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
|
1 | CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_x_dx_ddxSINNER), |
63 |
10/20✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
|
1 | CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_x_dx_ddxSINNER), |
64 |
10/20✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
|
2 | CONSTRUCT_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector, m_xSIN) { |
65 |
4/8✓ 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.
|
1 | Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS); |
66 | |||
67 | /* Commands. */ | ||
68 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | addCommand( |
69 | "getTimestep", | ||
70 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | makeDirectGetter(*this, &m_dt, |
71 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
2 | docDirectGetter("Control timestep [s ]", "double"))); |
72 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | addCommand("getSize", |
73 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | makeDirectGetter(*this, &m_x_size, |
74 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
2 | docDirectGetter("Size of the x signal", "int"))); |
75 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | addCommand("init", |
76 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | makeCommandVoid4(*this, &FilterDifferentiator::init, |
77 |
6/12✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
|
2 | docCommandVoid4("Initialize the filter.", |
78 | "Control timestep [s].", | ||
79 | "Size of the input signal x", | ||
80 | "Numerator of the filter", | ||
81 | "Denominator of the filter"))); | ||
82 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | addCommand("switch_filter", |
83 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | makeCommandVoid2( |
84 | *this, &FilterDifferentiator::switch_filter, | ||
85 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | docCommandVoid2("Switch Filter.", "Numerator of the filter", |
86 | "Denominator of the filter"))); | ||
87 | 1 | } | |
88 | |||
89 | /* --- COMMANDS ------------------------------------------------------ */ | ||
90 | /* --- COMMANDS ------------------------------------------------------ */ | ||
91 | /* --- COMMANDS ------------------------------------------------------ */ | ||
92 | 1 | void FilterDifferentiator::init(const double ×tep, const int &xSize, | |
93 | const Eigen::VectorXd &filter_numerator, | ||
94 | const Eigen::VectorXd &filter_denominator) { | ||
95 | 1 | m_x_size = xSize; | |
96 | 1 | m_dt = timestep; | |
97 | 1 | m_filter = | |
98 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | new CausalFilter(timestep, xSize, filter_numerator, filter_denominator); |
99 | |||
100 |
11/22✓ 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.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
|
1 | LOG("Filtering started with " |
101 | << "Numerator " << filter_numerator << std::endl | ||
102 | << "Denominator" << filter_denominator << std::endl); | ||
103 | 1 | return; | |
104 | } | ||
105 | |||
106 | ✗ | void FilterDifferentiator::switch_filter( | |
107 | const Eigen::VectorXd &filter_numerator, | ||
108 | const Eigen::VectorXd &filter_denominator) { | ||
109 | ✗ | LOG("Filter switched with " | |
110 | << "Numerator " << filter_numerator << std::endl | ||
111 | << "Denominator" << filter_denominator << std::endl | ||
112 | << "at time" << m_xSIN.getTime()); | ||
113 | ✗ | m_filter->switch_filter(filter_numerator, filter_denominator); | |
114 | } | ||
115 | |||
116 | /* --- SIGNALS ------------------------------------------------------ */ | ||
117 | /* --- SIGNALS ------------------------------------------------------ */ | ||
118 | /* --- SIGNALS ------------------------------------------------------ */ | ||
119 | |||
120 | 1 | DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector) { | |
121 | sotDEBUG(15) << "Compute x_dx inner signal " << iter << std::endl; | ||
122 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (s.size() != 3 * m_x_size) s.resize(3 * m_x_size); |
123 | // read encoders | ||
124 | 1 | const dynamicgraph::Vector &base_x = m_xSIN(iter); | |
125 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(base_x.size() == m_x_size); |
126 | 1 | m_filter->get_x_dx_ddx(base_x, s); | |
127 | 1 | return s; | |
128 | } | ||
129 | |||
130 | /// *************************************************************** /// | ||
131 | /// The following signals depend only on other inner signals, so they | ||
132 | /// just need to copy the interested part of the inner signal | ||
133 | /// they depend on. | ||
134 | /// *************************************************************** /// | ||
135 | |||
136 | 1 | DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector) { | |
137 | sotDEBUG(15) << "Compute x_filtered output signal " << iter << std::endl; | ||
138 | |||
139 | 1 | const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter); | |
140 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (s.size() != m_x_size) s.resize(m_x_size); |
141 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | s = x_dx_ddx.head(m_x_size); |
142 | 1 | return s; | |
143 | } | ||
144 | |||
145 | ✗ | DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) { | |
146 | sotDEBUG(15) << "Compute dx output signal " << iter << std::endl; | ||
147 | |||
148 | ✗ | const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter); | |
149 | ✗ | if (s.size() != m_x_size) s.resize(m_x_size); | |
150 | ✗ | s = x_dx_ddx.segment(m_x_size, m_x_size); | |
151 | ✗ | return s; | |
152 | } | ||
153 | |||
154 | ✗ | DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) { | |
155 | sotDEBUG(15) << "Compute ddx output signal " << iter << std::endl; | ||
156 | |||
157 | ✗ | const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter); | |
158 | ✗ | if (s.size() != m_x_size) s.resize(m_x_size); | |
159 | ✗ | s = x_dx_ddx.tail(m_x_size); | |
160 | ✗ | return s; | |
161 | } | ||
162 | |||
163 | ✗ | void FilterDifferentiator::display(std::ostream &os) const { | |
164 | ✗ | os << "FilterDifferentiator " << getName() << ":\n"; | |
165 | try { | ||
166 | ✗ | getProfiler().report_all(3, os); | |
167 | ✗ | } catch (ExceptionSignal e) { | |
168 | } | ||
169 | } | ||
170 | |||
171 | } // namespace sot | ||
172 | } // namespace dynamicgraph | ||
173 |