1 |
|
|
/* |
2 |
|
|
* Copyright 2018, |
3 |
|
|
* Julian Viereck |
4 |
|
|
* |
5 |
|
|
* CNRS/AIST |
6 |
|
|
* |
7 |
|
|
*/ |
8 |
|
|
|
9 |
|
|
#include <dynamic-graph/all-commands.h> |
10 |
|
|
#include <dynamic-graph/factory.h> |
11 |
|
|
|
12 |
|
|
#include <boost/function.hpp> |
13 |
|
|
#include <sot/core/exp-moving-avg.hh> |
14 |
|
|
#include <sot/core/factory.hh> |
15 |
|
|
|
16 |
|
|
namespace dg = ::dynamicgraph; |
17 |
|
|
|
18 |
|
|
/* ---------------------------------------------------------------------------*/ |
19 |
|
|
/* ------- GENERIC HELPERS -------------------------------------------------- */ |
20 |
|
|
/* ---------------------------------------------------------------------------*/ |
21 |
|
|
|
22 |
|
|
namespace dynamicgraph { |
23 |
|
|
namespace sot { |
24 |
|
|
|
25 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ExpMovingAvg, "ExpMovingAvg"); |
26 |
|
|
|
27 |
|
|
/* --------------------------------------------------------------------- */ |
28 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
29 |
|
|
/* --------------------------------------------------------------------- */ |
30 |
|
|
|
31 |
|
|
ExpMovingAvg::ExpMovingAvg(const std::string &n) |
32 |
|
|
: Entity(n), |
33 |
|
|
updateSIN(NULL, "ExpMovingAvg(" + n + ")::input(vector)::update"), |
34 |
|
|
refresherSINTERN("ExpMovingAvg(" + n + ")::intern(dummy)::refresher"), |
35 |
|
|
averageSOUT(boost::bind(&ExpMovingAvg::update, this, _1, _2), |
36 |
|
|
updateSIN << refresherSINTERN, |
37 |
|
|
"ExpMovingAvg(" + n + ")::output(vector)::average"), |
38 |
|
|
alpha(0.), |
39 |
|
|
init(false) { |
40 |
|
|
// Register signals into the entity. |
41 |
|
|
signalRegistration(updateSIN << averageSOUT); |
42 |
|
|
refresherSINTERN.setDependencyType(TimeDependency<int>::ALWAYS_READY); |
43 |
|
|
|
44 |
|
|
std::string docstring; |
45 |
|
|
// setAlpha |
46 |
|
|
docstring = |
47 |
|
|
"\n" |
48 |
|
|
" Set the alpha used to update the current value." |
49 |
|
|
"\n"; |
50 |
|
|
addCommand(std::string("setAlpha"), |
51 |
|
|
new ::dynamicgraph::command::Setter<ExpMovingAvg, double>( |
52 |
|
|
*this, &ExpMovingAvg::setAlpha, docstring)); |
53 |
|
|
} |
54 |
|
|
|
55 |
|
|
ExpMovingAvg::~ExpMovingAvg() {} |
56 |
|
|
|
57 |
|
|
/* --- COMPUTE ----------------------------------------------------------- */ |
58 |
|
|
/* --- COMPUTE ----------------------------------------------------------- */ |
59 |
|
|
/* --- COMPUTE ----------------------------------------------------------- */ |
60 |
|
|
|
61 |
|
|
void ExpMovingAvg::setAlpha(const double &alpha_) { |
62 |
|
|
assert(alpha <= 1. && alpha >= 0.); |
63 |
|
|
alpha = alpha_; |
64 |
|
|
} |
65 |
|
|
|
66 |
|
|
dynamicgraph::Vector &ExpMovingAvg::update(dynamicgraph::Vector &res, |
67 |
|
|
const int &inTime) { |
68 |
|
|
const dynamicgraph::Vector &update = updateSIN(inTime); |
69 |
|
|
|
70 |
|
|
if (init == false) { |
71 |
|
|
init = true; |
72 |
|
|
average = update; |
73 |
|
|
average.setZero(); |
74 |
|
|
res.resize(average.size()); |
75 |
|
|
} |
76 |
|
|
|
77 |
|
|
res = average = alpha * average + (1. - alpha) * update; |
78 |
|
|
return res; |
79 |
|
|
} |
80 |
|
|
|
81 |
|
|
} /* namespace sot */ |
82 |
|
|
} /* namespace dynamicgraph */ |