GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/core/gain-adaptive.hh Lines: 1 3 33.3 %
Date: 2023-03-13 12:09:37 Branches: 0 2 0.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * François Bleibel,
4
 * Olivier Stasse,
5
 *
6
 * CNRS/AIST
7
 *
8
 */
9
10
#ifndef __SOT_GAIN_ADAPTATIVE_HH__
11
#define __SOT_GAIN_ADAPTATIVE_HH__
12
13
/* --------------------------------------------------------------------- */
14
/* --- INCLUDE --------------------------------------------------------- */
15
/* --------------------------------------------------------------------- */
16
17
/* Matrix */
18
#include <dynamic-graph/linear-algebra.h>
19
20
/* SOT */
21
#include <dynamic-graph/all-signals.h>
22
#include <dynamic-graph/entity.h>
23
24
/* --------------------------------------------------------------------- */
25
/* --- API ------------------------------------------------------------- */
26
/* --------------------------------------------------------------------- */
27
28
#if defined(WIN32)
29
#if defined(gain_adaptive_EXPORTS)
30
#define SOTGAINADAPTATIVE_EXPORT __declspec(dllexport)
31
#else
32
#define SOTGAINADAPTATIVE_EXPORT __declspec(dllimport)
33
#endif
34
#else
35
#define SOTGAINADAPTATIVE_EXPORT
36
#endif
37
38
/* --------------------------------------------------------------------- */
39
/* --- CLASS ----------------------------------------------------------- */
40
/* --------------------------------------------------------------------- */
41
42
namespace dynamicgraph {
43
namespace sot {
44
45
/** Exponentially decreasing gain.
46
 * It follows the law \f[ g(e) = a \exp (-b ||e||) + c \f].
47
 *
48
 * The default values for
49
 * - \f$ a = 0   \f$,
50
 * - \f$ b = 0   \f$,
51
 * - \f$ c = 0.1 \f$.
52
 */
53
class SOTGAINADAPTATIVE_EXPORT GainAdaptive : public dynamicgraph::Entity {
54
 public: /* --- CONSTANTS --- */
55
  /* Default values. */
56
  static const double ZERO_DEFAULT;   // = 0.1
57
  static const double INFTY_DEFAULT;  // = 0.1
58
  static const double TAN_DEFAULT;    // = 1.
59
60
 public: /* --- ENTITY INHERITANCE --- */
61
  static const std::string CLASS_NAME;
62
  virtual void display(std::ostream &os) const;
63
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
64
65
 protected:
66
  /* Parameters of the adaptative-gain function:
67
   * lambda (x) = a * exp (-b*x) + c. */
68
  double coeff_a;
69
  double coeff_b;
70
  double coeff_c;
71
72
 public: /* --- CONSTRUCTORS ---- */
73
  GainAdaptive(const std::string &name);
74
  GainAdaptive(const std::string &name, const double &lambda);
75
  GainAdaptive(const std::string &name, const double &valueAt0,
76
               const double &valueAtInfty, const double &tanAt0);
77
78
 public: /* --- INIT --- */
79
2
  inline void init(void) { init(ZERO_DEFAULT, INFTY_DEFAULT, TAN_DEFAULT); }
80
  inline void init(const double &lambda) { init(lambda, lambda, 1.); }
81
  void init(const double &valueAt0, const double &valueAtInfty,
82
            const double &tanAt0);
83
  /** \brief Set the gain
84
   * by providing the value at 0, at \f$ \infty \f$ and the percentage of
85
   * accomplishment between both to be reached when the error is
86
   * \c errorReference.
87
   *
88
   * To visualize the curve of the gain versus the error, use
89
   * \code{.py}
90
   * from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
91
   * import numpy, matplotlib.pyplot as plt
92
   * g = GainAdaptive('g')
93
   * g.setByPoint(4.9, 0.001, 0.01, 0.1)
94
   *
95
   * errors = numpy.linspace(0, 0.1, 1000)
96
   * def compute(e):
97
   *     t = g.error.time + 1
98
   *     g.error.value = (e,)
99
   *     g.error.time = t
100
   *     g.gain.recompute(t)
101
   *     return g.gain.value
102
   *
103
   * gains = [ compute(e) for e in errors ]
104
   *
105
   * lg = plt.plot(errors, gains, 'r', label="Gain")
106
   * ld = plt.twinx().plot(errors, [ g*e for e,g in zip(errors,gains) ], 'b',
107
   *   label="Derivative")
108
   * lines = lg + ld
109
   * plt.legend(lines, [l.get_label() for l in lines])
110
   * plt.show()
111
   * \endcode
112
   */
113
  void initFromPassingPoint(const double &valueAt0, const double &valueAtInfty,
114
                            const double &errorReference,
115
                            const double &percentage);
116
  void forceConstant(void);
117
118
 public: /* --- SIGNALS --- */
119
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> errorSIN;
120
  dynamicgraph::SignalTimeDependent<double, int> gainSOUT;
121
122
 protected:
123
  double &computeGain(double &res, int t);
124
125
 private:
126
  void addCommands();
127
};
128
129
} /* namespace sot */
130
} /* namespace dynamicgraph */
131
132
#endif  // #ifndef __SOT_GAIN_ADAPTATIVE_HH__