Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpAdaptiveGain.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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
51const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
52const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666;
53const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666;
54
55/* --------------------------------------------------------------------------
56 */
57/* --- CONSTRUCTION ---------------------------------------------------------
58 */
59/* --------------------------------------------------------------------------
60 */
61
71vpAdaptiveGain::vpAdaptiveGain() : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
72{
73 this->initFromVoid();
74
75 return;
76}
77
84vpAdaptiveGain::vpAdaptiveGain(double c) : coeff_a(), coeff_b(), coeff_c(), lambda(1.) { initFromConstant(c); }
85
96vpAdaptiveGain::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
136
147void 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
204double 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
239double vpAdaptiveGain::value(double x) const
240{
241 this->lambda = this->value_const(x);
242
243 return lambda;
244}
245
253{
254 this->lambda = this->limitValue_const();
255
256 return lambda;
257}
258
259/* --------------------------------------------------------------------------
260 */
261/* --- ACCESSORS ------------------------------------------------------------
262 */
263/* --------------------------------------------------------------------------
264 */
265
280double vpAdaptiveGain::operator()(double x) const { return this->value(x); }
281
290double vpAdaptiveGain::operator()(void) const { return this->limitValue(); }
291
303double vpAdaptiveGain::operator()(const vpColVector &x) const { return this->value(x.infinityNorm()); }
304
305/* --------------------------------------------------------------------------
306 */
307/* --- OUTPUT ---------------------------------------------------------------
308 */
309/* --------------------------------------------------------------------------
310 */
311
319VISP_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.
double value_const(double x) const
double operator()(void) const
double setConstant(void)
static const double DEFAULT_LAMBDA_ZERO
double limitValue_const(void) const
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
void initFromVoid(void)
static const double DEFAULT_LAMBDA_INFINITY
static const double DEFAULT_LAMBDA_SLOPE
void initFromConstant(double c)
double limitValue(void) const
double value(double x) const
Implementation of column vector and the associated operations.
double infinityNorm() const