GCC Code Coverage Report

Directory: ./
File: src/filters/filter-differentiator.cpp
Date: 2025-05-13 12:28:21
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 &timestep, 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