Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpAdaptiveGain.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Adaptive gain.
33  *
34  * Authors:
35  * Nicolas Mansard
36  *
37  *****************************************************************************/
42 /* --- VISP --- */
43 #include <visp3/core/vpColVector.h>
44 #include <visp3/core/vpDebug.h>
45 #include <visp3/vs/vpAdaptiveGain.h>
46 
47 #include <cmath> // std::fabs
48 #include <iostream>
49 #include <limits> // numeric_limits
50 
51 const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
52 const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666;
53 const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666;
54 
55 /* --------------------------------------------------------------------------
56  */
57 /* --- CONSTRUCTION ---------------------------------------------------------
58  */
59 /* --------------------------------------------------------------------------
60  */
61 
71 vpAdaptiveGain::vpAdaptiveGain() : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
72 {
73  this->initFromVoid();
74 
75  return;
76 }
77 
84 vpAdaptiveGain::vpAdaptiveGain(double c) : coeff_a(), coeff_b(), coeff_c(), lambda(1.) { initFromConstant(c); }
85 
96 vpAdaptiveGain::vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
97  : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
98 {
99  initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
100 }
101 
102 /* --------------------------------------------------------------------------
103  */
104 /* --- INIT -----------------------------------------------------------------
105  */
106 /* --------------------------------------------------------------------------
107  */
108 
116 {
117  this->coeff_a = 0;
118  this->coeff_b = 1;
119  this->coeff_c = c;
120  return;
121 }
122 
131 {
134  return;
135 }
136 
147 void vpAdaptiveGain::initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
148 {
149  this->coeff_a = gain_at_zero - gain_at_infinity;
150  // if (0 == this ->coeff_a)
151  if (std::fabs(this->coeff_a) <= std::numeric_limits<double>::epsilon()) {
152  this->coeff_b = 0;
153  } else {
154  this->coeff_b = slope_at_zero / (this->coeff_a);
155  }
156  this->coeff_c = gain_at_infinity;
157 
158  return;
159 }
160 
161 /* --------------------------------------------------------------------------
162  */
163 /* --- MODIFICATOR ----------------------------------------------------------
164  */
165 /* --------------------------------------------------------------------------
166  */
167 
175 {
176  double res = this->coeff_a + this->coeff_c;
177 
178  this->coeff_a = 0;
179  this->coeff_b = 1;
180  this->coeff_c = res;
181 
182  return res;
183 }
184 
185 /* --------------------------------------------------------------------------
186  */
187 /* --- VALEUR ---------------------------------------------------------------
188  */
189 /* --------------------------------------------------------------------------
190  */
191 
204 double vpAdaptiveGain::value_const(double x) const
205 {
206  double res = this->coeff_a * exp(-this->coeff_b * x) + this->coeff_c;
207 
208  return res;
209 }
210 
219 {
220  double res = this->coeff_c;
221 
222  return res;
223 }
224 
239 double vpAdaptiveGain::value(double x) const
240 {
241  this->lambda = this->value_const(x);
242 
243  return lambda;
244 }
245 
252 double vpAdaptiveGain::limitValue(void) const
253 {
254  this->lambda = this->limitValue_const();
255 
256  return lambda;
257 }
258 
259 /* --------------------------------------------------------------------------
260  */
261 /* --- ACCESSORS ------------------------------------------------------------
262  */
263 /* --------------------------------------------------------------------------
264  */
265 
280 double vpAdaptiveGain::operator()(double x) const { return this->value(x); }
281 
290 double vpAdaptiveGain::operator()(void) const { return this->limitValue(); }
291 
303 double vpAdaptiveGain::operator()(const vpColVector &x) const { return this->value(x.infinityNorm()); }
304 
305 /* --------------------------------------------------------------------------
306  */
307 /* --- OUTPUT ---------------------------------------------------------------
308  */
309 /* --------------------------------------------------------------------------
310  */
311 
319 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda)
320 {
321  os << "Zero= " << lambda.coeff_a + lambda.coeff_c << "\tInf= " << lambda.coeff_c
322  << "\tSlope= " << lambda.coeff_a * lambda.coeff_b;
323 
324  return os;
325 }
Adaptive gain computation.
static const double DEFAULT_LAMBDA_ZERO
static const double DEFAULT_LAMBDA_INFINITY
static const double DEFAULT_LAMBDA_SLOPE
void initFromVoid(void)
double limitValue(void) const
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
double infinityNorm() const
double setConstant(void)
void initFromConstant(double c)
double operator()(void) const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpAdaptiveGain &lambda)
double value_const(double x) const
double value(double x) const
double limitValue_const(void) const