GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/filters/filter-differentiator.cpp Lines: 38 56 67.9 %
Date: 2023-03-13 12:09:37 Branches: 95 232 40.9 %

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 &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
      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