Skip to content

Commit af1667e

Browse files
jviereckOlivier Stasse
jviereck
authored and
Olivier Stasse
committed
Adding gradient ascent and expoential average mean entity
1 parent 82cd41f commit af1667e

File tree

6 files changed

+376
-4
lines changed

6 files changed

+376
-4
lines changed

‎include/sot/core/exp-moving-avg.hh‎

Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
/*
2+
* Copyright 2018,
3+
* Julian Viereck
4+
*
5+
* CNRS/AIST
6+
*
7+
* This file is part of sot-core.
8+
* sot-core is free software: you can redistribute it and/or
9+
* modify it under the terms of the GNU Lesser General Public License
10+
* as published by the Free Software Foundation, either version 3 of
11+
* the License, or (at your option) any later version.
12+
* sot-core is distributed in the hope that it will be
13+
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14+
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU Lesser General Public License for more details. You should
16+
* have received a copy of the GNU Lesser General Public License along
17+
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
18+
*/
19+
20+
#ifndef __SOT_EXPMOVINGAVG_H__
21+
#define __SOT_EXPMOVINGAVG_H__
22+
23+
/* --------------------------------------------------------------------- */
24+
/* --- INCLUDE --------------------------------------------------------- */
25+
/* --------------------------------------------------------------------- */
26+
27+
#include <dynamic-graph/entity.h>
28+
#include <dynamic-graph/signal-ptr.h>
29+
#include <dynamic-graph/signal-time-dependent.h>
30+
31+
namespace dg = ::dynamicgraph;
32+
33+
namespace dynamicgraph {
34+
namespace sot {
35+
36+
/* --------------------------------------------------------------------- */
37+
/* --- API ------------------------------------------------------------- */
38+
/* --------------------------------------------------------------------- */
39+
40+
#if defined (WIN32)
41+
# if defined (reader_EXPORTS)
42+
# define SOTEXPMOVINGAVG_EXPORT __declspec(dllexport)
43+
# else
44+
# define SOTEXPMOVINGAVG_EXPORT __declspec(dllimport)
45+
# endif
46+
#else
47+
# define SOTEXPMOVINGAVG_EXPORT
48+
#endif
49+
50+
/* --------------------------------------------------------------------- */
51+
/* --- TRACER ---------------------------------------------------------- */
52+
/* --------------------------------------------------------------------- */
53+
54+
using dynamicgraph::Entity;
55+
using dynamicgraph::SignalPtr;
56+
using dynamicgraph::SignalTimeDependent;
57+
58+
class SOTEXPMOVINGAVG_EXPORT ExpMovingAvg
59+
: public Entity
60+
{
61+
DYNAMIC_GRAPH_ENTITY_DECL();
62+
public:
63+
64+
SignalPtr< dg::Vector,int > updateSIN;
65+
SignalTimeDependent<int,int> refresherSINTERN;
66+
SignalTimeDependent<dg::Vector,int> averageSOUT;
67+
68+
public:
69+
ExpMovingAvg( const std::string& n );
70+
virtual ~ExpMovingAvg( void );
71+
72+
void setAlpha(const double& alpha_);
73+
74+
protected:
75+
76+
dg::Vector& update(dg::Vector& res, const int& inTime);
77+
78+
dg::Vector average;
79+
80+
double alpha;
81+
bool init;
82+
83+
};
84+
85+
} /* namespace sot */
86+
} /* namespace dynamicgraph */
87+
88+
#endif /* #ifndef __SOT_TRACER_H__ */
89+
90+

‎include/sot/core/gradient-ascent.hh‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
/*
2+
* Copyright 2018,
3+
* Julian Viereck
4+
*
5+
* CNRS/AIST
6+
*
7+
* This file is part of sot-core.
8+
* sot-core is free software: you can redistribute it and/or
9+
* modify it under the terms of the GNU Lesser General Public License
10+
* as published by the Free Software Foundation, either version 3 of
11+
* the License, or (at your option) any later version.
12+
* sot-core is distributed in the hope that it will be
13+
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14+
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU Lesser General Public License for more details. You should
16+
* have received a copy of the GNU Lesser General Public License along
17+
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
18+
*/
19+
20+
#ifndef __SOT_GRADIENTASCENT_H__
21+
#define __SOT_GRADIENTASCENT_H__
22+
23+
/* --------------------------------------------------------------------- */
24+
/* --- INCLUDE --------------------------------------------------------- */
25+
/* --------------------------------------------------------------------- */
26+
27+
#include <dynamic-graph/entity.h>
28+
#include <dynamic-graph/signal-ptr.h>
29+
#include <dynamic-graph/signal-time-dependent.h>
30+
31+
namespace dg = ::dynamicgraph;
32+
33+
namespace dynamicgraph {
34+
namespace sot {
35+
36+
/* --------------------------------------------------------------------- */
37+
/* --- API ------------------------------------------------------------- */
38+
/* --------------------------------------------------------------------- */
39+
40+
#if defined (WIN32)
41+
# if defined (reader_EXPORTS)
42+
# define SOTGRADIENTASCENT_EXPORT __declspec(dllexport)
43+
# else
44+
# define SOTGRADIENTASCENT_EXPORT __declspec(dllimport)
45+
# endif
46+
#else
47+
# define SOTGRADIENTASCENT_EXPORT
48+
#endif
49+
50+
/* --------------------------------------------------------------------- */
51+
/* --- TRACER ---------------------------------------------------------- */
52+
/* --------------------------------------------------------------------- */
53+
54+
using dynamicgraph::Entity;
55+
using dynamicgraph::SignalPtr;
56+
using dynamicgraph::SignalTimeDependent;
57+
58+
class SOTGRADIENTASCENT_EXPORT GradientAscent
59+
: public Entity
60+
{
61+
DYNAMIC_GRAPH_ENTITY_DECL();
62+
public:
63+
64+
SignalPtr< dg::Vector,int > gradientSIN;
65+
SignalPtr< double,int > learningRateSIN;
66+
SignalTimeDependent<int,int> refresherSINTERN;
67+
SignalTimeDependent<dg::Vector,int> valueSOUT;
68+
69+
public:
70+
GradientAscent( const std::string& n );
71+
virtual ~GradientAscent( void );
72+
73+
protected:
74+
75+
dg::Vector& update(dg::Vector& res, const int& inTime);
76+
77+
dg::Vector value;
78+
79+
double alpha;
80+
bool init;
81+
82+
};
83+
84+
} /* namespace sot */
85+
} /* namespace dynamicgraph */
86+
87+
#endif /* #ifndef __SOT_TRACER_H__ */
88+
89+

‎src/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,8 @@ SET(plugins
8989
tools/joint-trajectory-entity
9090
tools/latch
9191
tools/switch
92+
tools/exp-moving-avg
93+
tools/gradient-ascent
9294

9395
control/control-gr
9496
control/control-pd

‎src/dynamic_graph/sot/core/__init__.py‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,5 @@
1717
from visual_point_projecter import VisualPointProjecter
1818
from feature_visual_point import FeatureVisualPoint
1919
from kalman import Kalman
20-
21-
22-
23-
20+
from exp_moving_avg import ExpMovingAvg
21+
from gradient_ascent import GradientAscent

‎src/tools/exp-moving-avg.cpp‎

Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
/*
2+
* Copyright 2018,
3+
* Julian Viereck
4+
*
5+
* CNRS/AIST
6+
*
7+
* This file is part of sot-core.
8+
* sot-core is free software: you can redistribute it and/or
9+
* modify it under the terms of the GNU Lesser General Public License
10+
* as published by the Free Software Foundation, either version 3 of
11+
* the License, or (at your option) any later version.
12+
* sot-core is distributed in the hope that it will be
13+
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14+
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU Lesser General Public License for more details. You should
16+
* have received a copy of the GNU Lesser General Public License along
17+
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
18+
*/
19+
20+
#include <boost/function.hpp>
21+
22+
#include <dynamic-graph/all-commands.h>
23+
#include <dynamic-graph/factory.h>
24+
25+
#include <sot/core/factory.hh>
26+
#include <sot/core/exp-moving-avg.hh>
27+
28+
namespace dg = ::dynamicgraph;
29+
30+
/* ---------------------------------------------------------------------------*/
31+
/* ------- GENERIC HELPERS -------------------------------------------------- */
32+
/* ---------------------------------------------------------------------------*/
33+
34+
namespace dynamicgraph {
35+
namespace sot {
36+
37+
38+
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ExpMovingAvg,"ExpMovingAvg");
39+
40+
41+
/* --------------------------------------------------------------------- */
42+
/* --- CLASS ----------------------------------------------------------- */
43+
/* --------------------------------------------------------------------- */
44+
45+
46+
ExpMovingAvg::
47+
ExpMovingAvg( const std::string& n )
48+
:Entity(n)
49+
,updateSIN(NULL, "ExpMovingAvg(" + n + ")::input(vector)::update")
50+
,alpha(0.)
51+
,init(false)
52+
,refresherSINTERN( "ExpMovingAvg("+n+")::intern(dummy)::refresher" )
53+
,averageSOUT(
54+
boost::bind(&ExpMovingAvg::update,this,_1,_2),
55+
updateSIN << refresherSINTERN, "ExpMovingAvg(" + n + ")::output(vector)::average")
56+
{
57+
// Register signals into the entity.
58+
signalRegistration(updateSIN << averageSOUT);
59+
refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
60+
61+
std::string docstring;
62+
// setAlpha
63+
docstring =
64+
"\n"
65+
" Set the alpha used to update the current value."
66+
"\n";
67+
addCommand(std::string("setAlpha"),
68+
new ::dynamicgraph::command::Setter<ExpMovingAvg, double>
69+
(*this, &ExpMovingAvg::setAlpha, docstring));
70+
}
71+
72+
ExpMovingAvg::~ExpMovingAvg()
73+
{
74+
}
75+
76+
/* --- COMPUTE ----------------------------------------------------------- */
77+
/* --- COMPUTE ----------------------------------------------------------- */
78+
/* --- COMPUTE ----------------------------------------------------------- */
79+
80+
void ExpMovingAvg::setAlpha(const double& alpha_) {
81+
assert(alpha <= 1. && alpha >= 0.);
82+
alpha = alpha_;
83+
}
84+
85+
dynamicgraph::Vector& ExpMovingAvg::update(dynamicgraph::Vector& res,
86+
const int& inTime)
87+
{
88+
const dynamicgraph::Vector& update = updateSIN(inTime);
89+
90+
if (init == false) {
91+
init = true;
92+
average = update;
93+
average.setZero();
94+
res.resize(average.size());
95+
}
96+
97+
res = average = alpha * average + (1. - alpha) * update;
98+
return res;
99+
}
100+
101+
102+
103+
} /* namespace sot */
104+
} /* namespace dynamicgraph */

‎src/tools/gradient-ascent.cpp‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
/*
2+
* Copyright 2018,
3+
* Julian Viereck
4+
*
5+
* CNRS/AIST
6+
*
7+
* This file is part of sot-core.
8+
* sot-core is free software: you can redistribute it and/or
9+
* modify it under the terms of the GNU Lesser General Public License
10+
* as published by the Free Software Foundation, either version 3 of
11+
* the License, or (at your option) any later version.
12+
* sot-core is distributed in the hope that it will be
13+
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14+
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU Lesser General Public License for more details. You should
16+
* have received a copy of the GNU Lesser General Public License along
17+
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
18+
*/
19+
20+
#include <boost/function.hpp>
21+
22+
#include <dynamic-graph/all-commands.h>
23+
#include <dynamic-graph/factory.h>
24+
25+
#include <sot/core/factory.hh>
26+
#include <sot/core/gradient-ascent.hh>
27+
28+
namespace dg = ::dynamicgraph;
29+
30+
/* ---------------------------------------------------------------------------*/
31+
/* ------- GENERIC HELPERS -------------------------------------------------- */
32+
/* ---------------------------------------------------------------------------*/
33+
34+
namespace dynamicgraph {
35+
namespace sot {
36+
37+
38+
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GradientAscent,"GradientAscent");
39+
40+
41+
/* --------------------------------------------------------------------- */
42+
/* --- CLASS ----------------------------------------------------------- */
43+
/* --------------------------------------------------------------------- */
44+
45+
46+
GradientAscent::
47+
GradientAscent( const std::string& n )
48+
:Entity(n)
49+
,gradientSIN(NULL, "GradientAscent(" + n + ")::input(vector)::gradient")
50+
,learningRateSIN(NULL, "GradientAscent(" + n + ")::input(double)::learningRate")
51+
,init(false)
52+
,refresherSINTERN( "GradientAscent("+n+")::intern(dummy)::refresher" )
53+
,valueSOUT(
54+
boost::bind(&GradientAscent::update,this,_1,_2),
55+
gradientSIN << refresherSINTERN, "GradientAscent(" + n + ")::output(vector)::value")
56+
{
57+
// Register signals into the entity.
58+
signalRegistration(gradientSIN << learningRateSIN << valueSOUT);
59+
refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
60+
}
61+
62+
GradientAscent::~GradientAscent()
63+
{
64+
}
65+
66+
/* --- COMPUTE ----------------------------------------------------------- */
67+
/* --- COMPUTE ----------------------------------------------------------- */
68+
/* --- COMPUTE ----------------------------------------------------------- */
69+
70+
dynamicgraph::Vector& GradientAscent::update(dynamicgraph::Vector& res,
71+
const int& inTime)
72+
{
73+
const dynamicgraph::Vector& gradient = gradientSIN(inTime);
74+
const double& learningRate = learningRateSIN(inTime);
75+
76+
if (init == false) {
77+
init = true;
78+
value = gradient;
79+
value.setZero();
80+
res.resize(value.size());
81+
}
82+
83+
value += learningRate * gradient;
84+
res = value;
85+
return res;
86+
}
87+
88+
} /* namespace sot */
89+
} /* namespace dynamicgraph */

0 commit comments

Comments
 (0)