gain-adaptive.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 /* SOT */
12 
13 /* --------------------------------------------------------------------- */
14 /* --------------------------------------------------------------------- */
15 /* --------------------------------------------------------------------- */
17 
18 #include <sot/core/debug.hh>
20 #include <sot/core/factory.hh>
21 
22 using namespace dynamicgraph::sot;
23 using namespace dynamicgraph;
24 
26 
27 const double GainAdaptive::ZERO_DEFAULT = .1;
28 const double GainAdaptive::INFTY_DEFAULT = .1;
29 const double GainAdaptive::TAN_DEFAULT = 1;
30 
31 /* --------------------------------------------------------------------- */
32 /* --------------------------------------------------------------------- */
33 /* --------------------------------------------------------------------- */
34 
35 #define __SOT_GAIN_ADAPTATIVE_INIT \
36  Entity(name), coeff_a(0), coeff_b(0), coeff_c(0), \
37  errorSIN(NULL, "sotGainAdaptive(" + name + ")::input(vector)::error"), \
38  gainSOUT(boost::bind(&GainAdaptive::computeGain, this, _1, _2), \
39  errorSIN, \
40  "sotGainAdaptive(" + name + ")::output(double)::gain")
41 
43  using namespace ::dynamicgraph::command;
44  std::string docstring;
45  // Command SetConstant
46  docstring =
47  " \n"
48  " setConstant\n"
49  " Input:\n"
50  " floating point value: value at 0. Other values are set to"
51  "default.\n"
52  " \n";
53  addCommand("setConstant",
54  makeCommandVoid1(*this, &GainAdaptive::init, docstring));
55 
56  // Command Set
57  docstring =
58  " \n"
59  " set\n"
60  " Input:\n"
61  " floating point value: value at 0,\n"
62  " floating point value: value at infinity,\n"
63  " floating point value: value at slope,\n"
64  " \n";
65  addCommand("set", makeCommandVoid3(*this, &GainAdaptive::init, docstring));
66  docstring =
67  " \n"
68  " set from value at 0 and infinity, with a passing point\n"
69  " Input:\n"
70  " floating point value: value at 0,\n"
71  " floating point value: value at infinity,\n"
72  " floating point value: reference point,\n"
73  " floating point value: percentage at ref point.\n"
74  " \n";
75  addCommand(
76  "setByPoint",
78 }
79 
80 GainAdaptive::GainAdaptive(const std::string &name)
82  sotDEBUG(15) << "New gain <" << name << ">" << std::endl;
83  init();
85  addCommands();
86 }
87 
88 GainAdaptive::GainAdaptive(const std::string &name, const double &lambda)
90  init(lambda);
92  addCommands();
93 }
94 
95 GainAdaptive::GainAdaptive(const std::string &name, const double &valueAt0,
96  const double &valueAtInfty, const double &tanAt0)
98  init(valueAt0, valueAtInfty, tanAt0);
100  addCommands();
101 }
102 
103 void GainAdaptive::init(const double &valueAt0, const double &valueAtInfty,
104  const double &tanAt0) {
105  coeff_a = valueAt0 - valueAtInfty;
106  if (0 == coeff_a) {
107  coeff_b = 0;
108  } else {
109  coeff_b = tanAt0 / coeff_a;
110  }
111  coeff_c = valueAtInfty;
112 
113  return;
114 }
115 
116 /*
117  * The idea is to fix value at 0 and infinity. Now, we are looking for a smart
118  * way to chose the slope at 0 or the coeff B (it is more or less the same).
119  * I can imagine to way of using a passing point:
120  * - first, imposing a value gref at position xref: g(xref)=gref.
121  * In that case, B=-1/xref*log((gref-C)/A):
122  * gnuplot> A=1; C=.1; xref=.1; gref=.4; B=1/xref*log((gref-C)/A)
123  * gnuplot> plot [0:(A+C)/B*10][C-.1*(A-C):A+C+.1*(A-C)] A*exp(-B*x)+C,
124  * A+C-B*x
125  * - second solution: imposing to reach a percentage of raise at a given point.
126  * It is more or less the same as before, but with gref := C+p*A, for a given p.
127  * In that case, B=-1/xref*log(p) gnuplot> A=1; C=.1; xref=.1; p=.1;
128  * B=1/xref*log(p) gnuplot> plot [0:(A+C)/B*10][C-.1*(A-C):A+C+.1*(A-C)]
129  * A*exp(-B*x)+C, A+C-B*x
130  *
131  * The second solution is tried in the following.
132  */
133 void GainAdaptive::initFromPassingPoint(const double &valueAt0,
134  const double &valueAtInfty,
135  const double &xref,
136  const double &p) // gref )
137 {
138  coeff_c = valueAtInfty;
139  coeff_a = valueAt0 - valueAtInfty;
140  if (0 == coeff_a) {
141  coeff_b = 0;
142  } else {
143  // coeff_b = -1/xref*log( (gref-coeff_c)/coeff_a );
144  coeff_b = -1 / xref * log(p);
145  }
146 }
147 
149 
150 /* --------------------------------------------------------------------- */
151 /* --------------------------------------------------------------------- */
152 /* --------------------------------------------------------------------- */
153 
154 void GainAdaptive::display(std::ostream &os) const {
155  os << "Gain Adaptative " << getName();
156  try {
157  os << " = " << double(gainSOUT.accessCopy());
158  } catch (const ExceptionSignal &e) {
159  }
160  os << " (" << coeff_a << ";" << coeff_b << ";" << coeff_c << ") ";
161 }
162 
163 /* --------------------------------------------------------------------- */
164 /* --------------------------------------------------------------------- */
165 /* --------------------------------------------------------------------- */
166 double &GainAdaptive::computeGain(double &res, sigtime_t t) {
167  sotDEBUGIN(15);
168  const dynamicgraph::Vector &error = errorSIN(t);
169  const double norm = error.norm();
170  res = coeff_a * exp(-coeff_b * norm) + coeff_c;
171 
172  sotDEBUGOUT(15);
173  return res;
174 }
exception-signal.hh
dynamicgraph::sot::GainAdaptive::TAN_DEFAULT
static const double TAN_DEFAULT
Definition: gain-adaptive.hh:58
dynamicgraph::sot::GainAdaptive::computeGain
double & computeGain(double &res, sigtime_t t)
Definition: gain-adaptive.cpp:166
makeCommandVoid4
CommandVoid4< E, T1, T2, T3, T4 > * makeCommandVoid4(E &entity, boost::function< void(E *, const T1 &, const T2 &, const T3 &, const T4 &)> function, const std::string &docString)
factory.hh
dynamicgraph::sot::GainAdaptive::errorSIN
dynamicgraph::SignalPtr< dynamicgraph::Vector, sigtime_t > errorSIN
Definition: gain-adaptive.hh:119
dynamicgraph
exp
def exp(x)
dynamicgraph::command
dynamicgraph::Entity::name
std::string name
dynamicgraph::sot::GainAdaptive::initFromPassingPoint
void initFromPassingPoint(const double &valueAt0, const double &valueAtInfty, const double &errorReference, const double &percentage)
Set the gain by providing the value at 0, at and the percentage of accomplishment between both to be...
Definition: gain-adaptive.cpp:133
dynamicgraph::sot::GainAdaptive::GainAdaptive
GainAdaptive(const std::string &name)
Definition: gain-adaptive.cpp:80
dynamicgraph::sot::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture")
dynamicgraph::sot::GainAdaptive::INFTY_DEFAULT
static const double INFTY_DEFAULT
Definition: gain-adaptive.hh:57
dynamicgraph::Entity::getName
const std::string & getName() const
debug.hh
p
p
command-bind.h
dynamicgraph::sot::GainAdaptive::display
virtual void display(std::ostream &os) const
Definition: gain-adaptive.cpp:154
res
res
sotDEBUGOUT
#define sotDEBUGOUT(level)
Definition: debug.hh:215
dynamicgraph::sot::GainAdaptive::init
void init(void)
Definition: gain-adaptive.hh:79
dynamicgraph::sot::GainAdaptive::gainSOUT
dynamicgraph::SignalTimeDependent< double, sigtime_t > gainSOUT
Definition: gain-adaptive.hh:120
dynamicgraph::sigtime_t
int64_t sigtime_t
dynamicgraph::sot::GainAdaptive::addCommands
void addCommands()
Definition: gain-adaptive.cpp:42
dynamicgraph::sot::GainAdaptive::forceConstant
void forceConstant(void)
Definition: gain-adaptive.cpp:148
dynamicgraph::sot::GainAdaptive::ZERO_DEFAULT
static const double ZERO_DEFAULT
Definition: gain-adaptive.hh:56
sotDEBUGIN
#define sotDEBUGIN(level)
Definition: debug.hh:214
dynamicgraph::Vector
Eigen::VectorXd Vector
dynamicgraph::ExceptionSignal
dynamicgraph::sot::double
double
Definition: fir-filter.cpp:49
dynamicgraph::sot::GainAdaptive
Definition: gain-adaptive.hh:53
gain-adaptive.hh
dynamicgraph::sot::GainAdaptive::coeff_a
double coeff_a
Definition: gain-adaptive.hh:68
dynamicgraph::sot::GainAdaptive::coeff_b
double coeff_b
Definition: gain-adaptive.hh:69
__SOT_GAIN_ADAPTATIVE_INIT
#define __SOT_GAIN_ADAPTATIVE_INIT
Definition: gain-adaptive.cpp:35
dynamicgraph::sot
log
def log(x)
dynamicgraph::Entity::addCommand
void addCommand(const std::string &name, command::Command *command)
t
Transform3f t
makeCommandVoid3
CommandVoid3< E, T1, T2, T3 > * makeCommandVoid3(E &entity, boost::function< void(E *, const T1 &, const T2 &, const T3 &)> function, const std::string &docString)
dynamicgraph::Entity::signalRegistration
void signalRegistration(const SignalArray< sigtime_t > &signals)
dynamicgraph::sot::GainAdaptive::coeff_c
double coeff_c
Definition: gain-adaptive.hh:70
makeCommandVoid1
CommandVoid1< E, T > * makeCommandVoid1(E &entity, boost::function< void(const T &)> function, const std::string &docString)
compile.name
name
Definition: compile.py:23
sotDEBUG
#define sotDEBUG(level)
Definition: debug.hh:168


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:31