Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/task/gain-adaptive.cpp	Lines:	36	54	66.7 %
Date:	2023-03-13 12:09:37	Branches:	40	112	35.7 %


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

/* SOT */
#include <sot/core/gain-adaptive.hh>

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/command-bind.h>

#include <sot/core/debug.hh>
#include <sot/core/exception-signal.hh>
#include <sot/core/factory.hh>

using namespace dynamicgraph::sot;
using namespace dynamicgraph;

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GainAdaptive, "GainAdaptive");

const double GainAdaptive::ZERO_DEFAULT = .1;
const double GainAdaptive::INFTY_DEFAULT = .1;
const double GainAdaptive::TAN_DEFAULT = 1;

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#define __SOT_GAIN_ADAPTATIVE_INIT                                           \
  Entity(name), coeff_a(0), coeff_b(0), coeff_c(0),                          \
      errorSIN(NULL, "sotGainAdaptive(" + name + ")::input(vector)::error"), \
      gainSOUT(boost::bind(&GainAdaptive::computeGain, this, _1, _2),        \
               errorSIN,                                                     \
               "sotGainAdaptive(" + name + ")::output(double)::gain")

void GainAdaptive::addCommands() {
  using namespace ::dynamicgraph::command;
  std::string docstring;
  // Command SetConstant
  docstring =
      "    \n"
      "    setConstant\n"
      "      Input:\n"
      "        floating point value: value at 0. Other values are set to"
      "default.\n"
      "    \n";
  addCommand("setConstant",
             makeCommandVoid1(*this, &GainAdaptive::init, docstring));

  // Command Set
  docstring =
      "    \n"
      "    set\n"
      "      Input:\n"
      "        floating point value: value at 0,\n"
      "        floating point value: value at infinity,\n"
      "        floating point value: value at slope,\n"
      "    \n";
  addCommand("set", makeCommandVoid3(*this, &GainAdaptive::init, docstring));
  docstring =
      "    \n"
      "    set from value at 0 and infinity, with a passing point\n"
      "      Input:\n"
      "        floating point value: value at 0,\n"
      "        floating point value: value at infinity,\n"
      "        floating point value: reference point,\n"
      "        floating point value: percentage at ref point.\n"
      "    \n";
  addCommand(
      "setByPoint",
      makeCommandVoid4(*this, &GainAdaptive::initFromPassingPoint, docstring));
}

GainAdaptive::GainAdaptive(const std::string &name)
    : __SOT_GAIN_ADAPTATIVE_INIT {
  sotDEBUG(15) << "New gain <" << name << ">" << std::endl;
  init();
  Entity::signalRegistration(gainSOUT << errorSIN);
  addCommands();
}

GainAdaptive::GainAdaptive(const std::string &name, const double &lambda)
    : __SOT_GAIN_ADAPTATIVE_INIT {
  init(lambda);
  Entity::signalRegistration(gainSOUT);
  addCommands();
}

GainAdaptive::GainAdaptive(const std::string &name, const double &valueAt0,
                           const double &valueAtInfty, const double &tanAt0)
    : __SOT_GAIN_ADAPTATIVE_INIT {
  init(valueAt0, valueAtInfty, tanAt0);
  Entity::signalRegistration(gainSOUT);
  addCommands();
}

void GainAdaptive::init(const double &valueAt0, const double &valueAtInfty,
                        const double &tanAt0) {
  coeff_a = valueAt0 - valueAtInfty;
  if (0 == coeff_a) {
    coeff_b = 0;
  } else {
    coeff_b = tanAt0 / coeff_a;
  }
  coeff_c = valueAtInfty;

  return;
}

/*
 * The idea is to fix value at 0 and infinity. Now, we are looking for a smart
 * way to chose the slope at 0 or the coeff B (it is more or less the same).
 * I can imagine to way of using a passing point:
 *  - first, imposing a value gref at position xref: g(xref)=gref.
 * In that case, B=-1/xref*log((gref-C)/A):
 *     gnuplot> A=1; C=.1; xref=.1; gref=.4; B=1/xref*log((gref-C)/A)
 *     gnuplot> plot [0:(A+C)/B*10][C-.1*(A-C):A+C+.1*(A-C)] A*exp(-B*x)+C,
 * A+C-B*x
 *  - second solution: imposing to reach a percentage of raise at a given point.
 * It is more or less the same as before, but with gref := C+p*A, for a given p.
 * In that case, B=-1/xref*log(p) gnuplot> A=1; C=.1; xref=.1; p=.1;
 * B=1/xref*log(p) gnuplot> plot [0:(A+C)/B*10][C-.1*(A-C):A+C+.1*(A-C)]
 * A*exp(-B*x)+C, A+C-B*x
 *
 * The second solution is tried in the following.
 */
void GainAdaptive::initFromPassingPoint(const double &valueAt0,
                                        const double &valueAtInfty,
                                        const double &xref,
                                        const double &p)  // gref )
{
  coeff_c = valueAtInfty;
  coeff_a = valueAt0 - valueAtInfty;
  if (0 == coeff_a) {
    coeff_b = 0;
  } else {
    // coeff_b = -1/xref*log( (gref-coeff_c)/coeff_a );
    coeff_b = -1 / xref * log(p);
  }
}

void GainAdaptive::forceConstant(void) { coeff_a = 0; }

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

void GainAdaptive::display(std::ostream &os) const {
  os << "Gain Adaptative " << getName();
  try {
    os << " = " << double(gainSOUT.accessCopy());
  } catch (ExceptionSignal e) {
  }
  os << " (" << coeff_a << ";" << coeff_b << ";" << coeff_c << ") ";
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
double &GainAdaptive::computeGain(double &res, int t) {
  sotDEBUGIN(15);
  const dynamicgraph::Vector &error = errorSIN(t);
  const double norm = error.norm();
  res = coeff_a * exp(-coeff_b * norm) + coeff_c;

  sotDEBUGOUT(15);
  return res;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010,
3			* François Bleibel,
4			* Olivier Stasse,
5			*
6			* CNRS/AIST
7			*
8			*/
9
10			/* SOT */
11			#include <sot/core/gain-adaptive.hh>
12
13			/* --------------------------------------------------------------------- */
14			/* --------------------------------------------------------------------- */
15			/* --------------------------------------------------------------------- */
16			#include <dynamic-graph/command-bind.h>
17
18			#include <sot/core/debug.hh>
19			#include <sot/core/exception-signal.hh>
20			#include <sot/core/factory.hh>
21
22			using namespace dynamicgraph::sot;
23			using namespace dynamicgraph;
24
25			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GainAdaptive, "GainAdaptive");
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
42		3	void GainAdaptive::addCommands() {
43			using namespace ::dynamicgraph::command;
44		3	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	✓✗	3	" \n";
53	✓✗✓✗	3	addCommand("setConstant",
54	✓✗	3	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	✓✗	3	" \n";
65	✓✗✓✗ ✓✗	3	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	✓✗	3	" \n";
75	✓✗✓✗	3	addCommand(
76			"setByPoint",
77	✓✗	3	makeCommandVoid4(*this, &GainAdaptive::initFromPassingPoint, docstring));
78		3	}
79
80		2	GainAdaptive::GainAdaptive(const std::string &name)
81	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	: __SOT_GAIN_ADAPTATIVE_INIT {
82			sotDEBUG(15) << "New gain <" << name << ">" << std::endl;
83	✓✗	2	init();
84	✓✗✓✗	2	Entity::signalRegistration(gainSOUT << errorSIN);
85	✓✗	2	addCommands();
86		2	}
87
88			GainAdaptive::GainAdaptive(const std::string &name, const double &lambda)
89			: __SOT_GAIN_ADAPTATIVE_INIT {
90			init(lambda);
91			Entity::signalRegistration(gainSOUT);
92			addCommands();
93			}
94
95		1	GainAdaptive::GainAdaptive(const std::string &name, const double &valueAt0,
96		1	const double &valueAtInfty, const double &tanAt0)
97	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	1	: __SOT_GAIN_ADAPTATIVE_INIT {
98	✓✗	1	init(valueAt0, valueAtInfty, tanAt0);
99	✓✗✓✗	1	Entity::signalRegistration(gainSOUT);
100	✓✗	1	addCommands();
101		1	}
102
103		3	void GainAdaptive::init(const double &valueAt0, const double &valueAtInfty,
104			const double &tanAt0) {
105		3	coeff_a = valueAt0 - valueAtInfty;
106	✓✓	3	if (0 == coeff_a) {
107		2	coeff_b = 0;
108			} else {
109		1	coeff_b = tanAt0 / coeff_a;
110			}
111		3	coeff_c = valueAtInfty;
112
113		3	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)/B10][C-.1(A-C):A+C+.1(A-C)] Aexp(-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/xreflog(p) gnuplot> plot [0:(A+C)/B10][C-.1(A-C):A+C+.1(A-C)]
129			* Aexp(-Bx)+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
148			void GainAdaptive::forceConstant(void) { coeff_a = 0; }
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 (ExceptionSignal e) {
159			}
160			os << " (" << coeff_a << ";" << coeff_b << ";" << coeff_c << ") ";
161			}
162
163			/* --------------------------------------------------------------------- */
164			/* --------------------------------------------------------------------- */
165			/* --------------------------------------------------------------------- */
166		1	double &GainAdaptive::computeGain(double &res, int t) {
167			sotDEBUGIN(15);
168		1	const dynamicgraph::Vector &error = errorSIN(t);
169		1	const double norm = error.norm();
170		1	res = coeff_a * exp(-coeff_b * norm) + coeff_c;
171
172			sotDEBUGOUT(15);
173		1	return res;
174			}