exp-moving-avg.cpp 2.44 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
/*
 * Copyright 2018,
 * Julian Viereck
 *
 * CNRS/AIST
 *
 */

#include <boost/function.hpp>

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>

#include <sot/core/factory.hh>
#include <sot/core/exp-moving-avg.hh>

namespace dg = ::dynamicgraph;

/* ---------------------------------------------------------------------------*/
/* ------- GENERIC HELPERS -------------------------------------------------- */
/* ---------------------------------------------------------------------------*/

namespace dynamicgraph {
  namespace sot {


DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ExpMovingAvg,"ExpMovingAvg");


/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */


ExpMovingAvg::
ExpMovingAvg( const std::string& n )
  :Entity(n)
38
39
40
41
42
43
44
  ,updateSIN(NULL, "ExpMovingAvg(" + n + ")::input(vector)::update")   
  ,refresherSINTERN( "ExpMovingAvg("+n+")::intern(dummy)::refresher"  )
  ,averageSOUT(
	       boost::bind(&ExpMovingAvg::update,this,_1,_2),
	       updateSIN << refresherSINTERN, "ExpMovingAvg(" + n + ")::output(vector)::average")
  ,alpha(0.)
  ,init(false)
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
{
  // Register signals into the entity.
  signalRegistration(updateSIN << averageSOUT);
  refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );

  std::string docstring;
  // setAlpha
  docstring =
    "\n"
    "    Set the alpha used to update the current value."
    "\n";
  addCommand(std::string("setAlpha"),
	     new ::dynamicgraph::command::Setter<ExpMovingAvg, double>
	     (*this, &ExpMovingAvg::setAlpha, docstring));
}

ExpMovingAvg::~ExpMovingAvg()
{
}

/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */

void ExpMovingAvg::setAlpha(const double& alpha_) {
  assert(alpha <= 1. && alpha >= 0.);
  alpha = alpha_;
}

dynamicgraph::Vector& ExpMovingAvg::update(dynamicgraph::Vector& res,
						 const int& inTime)
{
  const dynamicgraph::Vector& update = updateSIN(inTime);

  if (init == false) {
    init = true;
    average = update;
    average.setZero();
    res.resize(average.size());
  }

  res = average = alpha * average + (1. - alpha) * update;
  return res;
}



  } /* namespace sot */
} /* namespace dynamicgraph */