gain-hyperbolic.cpp

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
/* SOT */
#include <sot/core/gain-hyperbolic.hh>
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#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(GainHyperbolic, "GainHyperbolic");
 
const double GainHyperbolic::ZERO_DEFAULT = .1;
const double GainHyperbolic::INFTY_DEFAULT = .1;
const double GainHyperbolic::TAN_DEFAULT = 1;
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#define __SOT_GAIN_HYPERBOLIC_INIT                                             \
  Entity(name), coeff_a(0), coeff_b(0), coeff_c(0), coeff_d(0),                \
      errorSIN(NULL, "sotGainHyperbolic(" + name + ")::input(vector)::error"), \
      gainSOUT(boost::bind(&GainHyperbolic::computeGain, this, _1, _2),        \
               errorSIN,                                                       \
               "sotGainHyperbolic(" + name + ")::output(double)::gain")
 
GainHyperbolic::GainHyperbolic(const std::string &name)
    : __SOT_GAIN_HYPERBOLIC_INIT {
  sotDEBUG(15) << "New gain <" << name << ">" << std::endl;
  init();
  Entity::signalRegistration(gainSOUT << errorSIN);
}
 
GainHyperbolic::GainHyperbolic(const std::string &name, const double &lambda)
    : __SOT_GAIN_HYPERBOLIC_INIT {
  init(lambda);
  Entity::signalRegistration(gainSOUT);
}
 
GainHyperbolic::GainHyperbolic(const std::string &name, const double &valueAt0,
                               const double &valueAtInfty, const double &tanAt0,
                               const double &decal0)
    : __SOT_GAIN_HYPERBOLIC_INIT {
  init(valueAt0, valueAtInfty, tanAt0, decal0);
  Entity::signalRegistration(gainSOUT);
}
 
void GainHyperbolic::init(const double &valueAt0, const double &valueAtInfty,
                          const double &tanAt0, const double &decal0) {
  coeff_a = valueAt0 - valueAtInfty;
  if (0 == coeff_a) {
    coeff_b = 0;
  } else {
    coeff_b = tanAt0 / coeff_a / 2;
  }
  coeff_c = valueAtInfty;
  coeff_d = decal0;
 
  return;
}
 
void GainHyperbolic::forceConstant(void) { coeff_a = 0; }
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
void GainHyperbolic::display(std::ostream &os) const {
  os << "Gain Hyperbolic " << getName();
  try {
    os << " = " << double(gainSOUT.accessCopy());
  } catch (const ExceptionSignal &e) {
  }
  // os <<" ("<<coeff_a<<";"<<coeff_b<<";"<<coeff_c<<coeff_d<<") ";
  os << " (" << coeff_a << ".exp(-" << coeff_b << "(x-" << coeff_d << "))+"
     << coeff_c;
}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
double &GainHyperbolic::computeGain(double &res, sigtime_t t) {
  sotDEBUGIN(15);
  const dynamicgraph::Vector &error = errorSIN(t);
  const double norm = error.norm();
  res = coeff_a * .5 * (tanh(-coeff_b * (norm - coeff_d)) + 1) + coeff_c;
 
  sotDEBUGOUT(15);
  return res;
}