GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector), |
61 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_OUT(x_filtered, dynamicgraph::Vector, m_x_dx_ddxSINNER), |
62 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_x_dx_ddxSINNER), |
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_x_dx_ddxSINNER), |
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
9 |
CONSTRUCT_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector, m_xSIN) { |
65 |
✓✗✓✗ ✓✗✓✗ |
1 |
Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS); |
66 |
|||
67 |
/* Commands. */ |
||
68 |
✓✗✓✗ |
1 |
addCommand( |
69 |
"getTimestep", |
||
70 |
✓✗ | 1 |
makeDirectGetter(*this, &m_dt, |
71 |
✓✗✓✗ ✓✗ |
2 |
docDirectGetter("Control timestep [s ]", "double"))); |
72 |
✓✗✓✗ |
1 |
addCommand("getSize", |
73 |
✓✗ | 1 |
makeDirectGetter(*this, &m_x_size, |
74 |
✓✗✓✗ ✓✗ |
2 |
docDirectGetter("Size of the x signal", "int"))); |
75 |
✓✗✓✗ |
1 |
addCommand("init", |
76 |
✓✗ | 1 |
makeCommandVoid4(*this, &FilterDifferentiator::init, |
77 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
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 |
✓✗✓✗ |
1 |
addCommand("switch_filter", |
83 |
✓✗ | 1 |
makeCommandVoid2( |
84 |
*this, &FilterDifferentiator::switch_filter, |
||
85 |
✓✗✓✗ ✓✗✓✗ |
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 |
new CausalFilter(timestep, xSize, filter_numerator, filter_denominator); |
99 |
|||
100 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
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 |
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 |
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 |
if (s.size() != m_x_size) s.resize(m_x_size); |
141 |
✓✗ | 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 |
Generated by: GCOVR (Version 4.2) |