invertmotorcontroller.h

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *                                                                         *
 *   ANY COMMERCIAL USE FORBIDDEN!                                         *
 *   LICENSE:                                                              *
 *   This work is licensed under the Creative Commons                      *
 *   Attribution-NonCommercial-ShareAlike 2.5 License. To view a copy of   *
 *   this license, visit http://creativecommons.org/licenses/by-nc-sa/2.5/ *
 *   or send a letter to Creative Commons, 543 Howard Street, 5th Floor,   *
 *   San Francisco, California, 94105, USA.                                *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.                  *
 *                                                                         *
 *   $Log: invertmotorcontroller.h,v $
 *   Revision 1.23  2006/08/02 09:30:16  martius
 *   virtualized new calc functions
 *   calcErrorFactor added
 *
 *   Revision 1.22  2006/07/20 17:14:34  martius
 *   removed std namespace from matrix.h
 *   storable interface
 *   abstract model and invertablemodel as superclasses for networks
 *
 *   Revision 1.21  2006/07/18 15:17:16  martius
 *   buffer operations like delayed values and smoothed values moved to invertmotorcontroller
 *
 *   Revision 1.20  2006/07/18 14:47:15  martius
 *   new regularisation of g' and g''/g'
 *   g''/g' uses the same clipped z which used for g'
 *   sqashing function with variable squash size
 *
 *   Revision 1.19  2006/07/14 12:23:58  martius
 *   selforg becomes HEAD
 *
 *   Revision 1.17.6.10  2006/07/10 13:05:16  martius
 *   NON-COMMERICAL LICENSE added to controllers
 *
 *                                                                         *
 ***************************************************************************/

#ifndef __INVERTMOTORCONTROLLER_H
#define __INVERTMOTORCONTROLLER_H

#include "abstractcontroller.h"
#include "controller_misc.h"
#include <stdlib.h>
#include <string.h>


/**
 * Abstract class (interface) for robot controller that use direct matrix inversion and
 * a time loop in motor domain
 * 
 * Implements standart configureable interface for some useful parameters like  epsC, epsA, s4avg ...
 */
class InvertMotorController : public AbstractController {
public:
  InvertMotorController( unsigned short buffersize ,
                         const std::string& name, const std::string& revision)
    : AbstractController(name, revision){
    this->buffersize = buffersize;
    epsC=0.1;    // base learning rate
    desens=0.0; // name form desensitization
    s4delay=1;
    s4avg=1;
    steps=1;  
    epsA=0.1; // learning rate factor for model learning
    dampA=0;  // no damping
    factorB=0.2; // additional learning rate factor for model bias 
    zetaupdate=0; // don't use the zeta update line
    squashSize=0.01; // maximum size of update components
    logaE=0; // don't use logarithmic error ( ln(v^T*v) )
    rootE=0; // don't use logarithmic error ( (v^T*v)^(1/2) )
    relativeE=0; // don't use relative modelling error 
    adaptRate=0.005; // factor used for adapting the lerning rates.
    nomUpdate=0.005; // size of nominal update in respect to matrix norm 
    noiseB = 0.001;   // noise that is added to bias update (before factorB is applied)
    noiseY = 0.1;   // noise that is added to motor values
    teacher = 5;      // factor for teaching signal
    t=0;
    initialised = false;
  }

  /***** CONFIGURABLE ******/

  virtual paramval getParam(const paramkey& key) const{
    if(key == "epsC") return epsC; 
    else if(key == "epsA") return epsA; 
    else if(key == "dampA") return dampA;
    else if(key == "adaptrate") return adaptRate;
    else if(key == "nomupdate") return nomUpdate;
    else if(key == "desens") return desens; 
    else if(key == "s4delay") return s4delay; 
    else if(key == "s4avg") return s4avg; 
    else if(key == "steps") return steps; 
    else if(key == "zetaupdate") return zetaupdate; 
    else if(key == "squashsize") return squashSize; 
    else if(key == "logaE") return logaE; 
    else if(key == "rootE") return rootE;
    else if(key == "relativeE") return relativeE;
    else if(key == "factorB") return factorB; 
    else if(key == "noiseB") return noiseB;
    else if(key == "noiseY") return noiseY;
    else if(key == "teacher") return teacher;
    else  return AbstractController::getParam(key);
  }

  virtual bool setParam(const paramkey& key, paramval val){
    if(key == "epsC") epsC=val;
    else if(key == "epsA") epsA=val;
    else if(key == "dampA") dampA=val;
    else if(key == "adaptrate") adaptRate=val;
    else if(key == "nomupdate") nomUpdate=val;
    else if(key == "desens") desens=val;
    else if(key == "s4delay") s4delay=val; 
    else if(key == "s4avg") s4avg=val;
    else if(key == "steps") steps=val;
    else if(key == "zetaupdate") zetaupdate=val;
    else if(key == "squashsize") squashSize=val;
    else if(key == "logaE") logaE=(short)val;
    else if(key == "rootE") rootE=(short)val;
    else if(key == "relativeE") relativeE=(short)val;
    else if(key == "factorB") factorB=val;
    else if(key == "noiseB") noiseB=val;
    else if(key == "noiseY") noiseY=val;
    else if(key == "teacher") teacher=val;
    else return AbstractController::setParam(key, val);
    //    else fprintf(stderr, "parameter %s unknown\n", key);
    return true;
  }

  virtual paramlist getParamList() const{
    paramlist list;
    list.push_back(std::pair<paramkey, paramval> ("epsA", epsA));
    list.push_back(std::pair<paramkey, paramval> ("epsC", epsC));
    list.push_back(std::pair<paramkey, paramval> ("dampA", dampA));
    list.push_back(std::pair<paramkey, paramval> ("adaptrate", adaptRate));
    list.push_back(std::pair<paramkey, paramval> ("nomupdate", nomUpdate));
    list.push_back(std::pair<paramkey, paramval> ("desens", desens));
    list.push_back(std::pair<paramkey, paramval> ("s4delay", s4delay ));
    list.push_back(std::pair<paramkey, paramval> ("s4avg", s4avg));
    list.push_back(std::pair<paramkey, paramval> ("steps", steps));
    list.push_back(std::pair<paramkey, paramval> ("zetaupdate", zetaupdate));
    list.push_back(std::pair<paramkey, paramval> ("squashsize", squashSize));
    list.push_back(std::pair<paramkey, paramval> ("logaE", logaE));
    list.push_back(std::pair<paramkey, paramval> ("rootE", rootE));
    list.push_back(std::pair<paramkey, paramval> ("relativeE", relativeE));
    list.push_back(std::pair<paramkey, paramval> ("factorB", factorB));
    list.push_back(std::pair<paramkey, paramval> ("noiseB", noiseB));
    list.push_back(std::pair<paramkey, paramval> ("noiseY", noiseY));
    list.push_back(std::pair<paramkey, paramval> ("teacher", teacher));
    return list;
  }

protected:
  paramval epsC; //< learning rate factor for controller learning
  paramval desens;
  paramval s4delay; //< number of timesteps of delay in the SML 
  paramval s4avg; //< number of timesteps used for smoothing the controller output values 
  paramval steps; //< number of timesteps is used for controller learning
  paramval epsA; //< learning rate factor for model learning
  paramval factorB; //< additional learning rate factor for model bias 
  paramval zetaupdate;
  paramval dampA; //< damping term for model (0: no damping, 0.1 high damping)
  short logaE;  //< logarithmic error is used for learning 1: controller 2: model 3: both
  short rootE;  //< root error is used for learning 1: controller 2: model 3: both
  short relativeE; //< if not 0: a relative error signal is used (xsi is normalised in respect to |y|)

  paramval squashSize; //< size of the box, where the parameter updates are clipped to
  paramval adaptRate; //< adaptation rate for learning rate adaptation
  paramval nomUpdate; //< nominal update of controller in respect to matrix norm
  paramval noiseB;    //< size of the additional noise for model bias B
  paramval noiseY;    //< size of the additional noise for motor values
  paramval teacher; //< factor for teaching signal

  int t;
  unsigned short buffersize;
  std::string name;
  bool initialised;

protected:
  // put new value in ring buffer
  void putInBuffer(matrix::Matrix* buffer, const matrix::Matrix& vec, int delay = 0){
    buffer[(t-delay)%buffersize] = vec;
  }

  /// calculate delayed values
  virtual matrix::Matrix calculateDelayedValues(const matrix::Matrix* buffer, 
                                                  int number_steps_of_delay_){
    // number_steps_of_delay must not be smaller than buffersize
    assert ((unsigned)number_steps_of_delay_ < buffersize);  
    return buffer[(t - number_steps_of_delay_) % buffersize];
  };
  
  /// calculate time-smoothed values 
  virtual matrix::Matrix calculateSmoothValues(const matrix::Matrix* buffer, 
                                                 int number_steps_for_averaging_){
    // number_steps_for_averaging_ must not be larger than buffersize
    assert ((int)number_steps_for_averaging_ <= buffersize);
    
    matrix::Matrix result(buffer[t % buffersize]);
    for (int k = 1; k < number_steps_for_averaging_; k++) {
      result += buffer[(t - k) % buffersize];
    }
    result *= 1/((double) (number_steps_for_averaging_)); // scalar multiplication
    return result;
  };

  /// calculates the error_factor for either logarithmic (E=ln(e^T*e)) or square (E=sqrt(e^t*e)) error
  virtual double calcErrorFactor(const matrix::Matrix& e, bool loga, bool root) {
    double error_factor = 1;
    if (loga){   // using logarithmic error E=ln(v^T*v)
      error_factor= 1/(e.multTM().val(0,0)+0.000001)*0.01; // factor 1/100 for normalising (empirically)
    }
    if (root){  // using root error E=(v^T*v)^(1/2) 
      error_factor= 1/sqrt(e.multTM().val(0,0)+0.000001)*0.1; // factor 1/10 for normalising (empirically)
    }  
    return error_factor;
  }
  
  
  /// neuron transfer function
  static double g(double z)
  {
    return tanh(z);
  };

  /// first dervative
  static double g_s(double z)
  {
    double k=tanh(z); 
    return 1.025 - k*k; 
    // return 1/(1+z*z);    // softer
    //return 1/(1+log(1+z*z)); // even softer
  };

  /// inverse of the first derivative
  static double g_s_inv(double z)
  {
    double k=tanh(z);
    return 1/(1.025 - k*k);
    // return 1+z*z; // softer
    //return 1+log(1+z*z); // even softer
  };


  /// an exact formula for inversion if neuron.
  //  zeta = (1/(g'(z , xsi)) * xsi
  static double g_s(double z, double xsi) {
    double Z = clip(z, -3.0, 3.0) + clip(xsi, -1.0, 1.0);  
    double k=tanh(Z);  // approximation with Mittelwertsatz 
    return 1 - k*k;      
    /*   exact formula, which does not work for some reason: */
    /*   double s = - fabs(xsi) * sign(z); */
    /*     // for s some assumption have to be hold s must be in the inverval (- 2/(e^-2|z|+1), 2/(e^-2|z|+1)) without 0. */
    /*     // in case of s=0 we return g' */
    /*     if(fabs(s) < 0.0001) return g_s(z); // avoid singularities */
    /*     else{ */
    /*       double e_2z = exp(2*z); */
    /*       double limit = 1.9/(exp(-2*fabs(z)) +1); */
    /*       s = clip(s, -limit, limit); */
    /*       return 2 * s * 1/(log( (2+(1/e_2z+1)*s) / (2 - (e_2z+1) * s) )); */
    /*     } */
  };

  /// an exact formula for g''/g'
  static double g_2s_div_s(double z, double xsi) { 
    // for consistency reasons we use the same clipped z as for g'.
    double Z = clip(z, -3.0, 3.0) + clip(xsi, -1.0, 1.0);  
    // approximation with Mittelwertsatz (z is clipped)
    return -2*g(Z);

    // return -2*g(z);  // <- this is actually exact
    
/*   exact formula, which does not work for some reason: */
/*     double s = - fabs(xsi) * sign(z); */
/*     // for s some assumption have to be hold s must be in the inverval (- 2/(e^-2|z|+1), 2/(e^-2|z|+1)) without 0. */
/*     // in case of s=0 we return g''/g' = -2 g */
/*     if(fabs(s) < 0.0001) { */
/*       return -2*g(z); // avoid singularities */
/*     } else{ */
/*       double e_m2z = exp(-2*z); */
/*       double limit = 1.9/(exp(-2*fabs(z)) +1 ); */
/*       s = clip(s, -limit, limit); */
/*       return q_s(z,xsi) * ( ( sqr(1+e_m2z)*s + 2*(1 - sqr(e_m2z)) ) */
/*                          / ( sqr(1+e_m2z)*s*s + 2*(1-sqr(e_m2z))*s - 4*e_m2z ) ); */
/*     } */
  };

  /// squashing function (-0.1 to 0.1), to protect against to large weight updates
  static double squash(double z)
  {
    return z < -0.1 ? -0.1 : ( z > 0.1 ? 0.1 : z );
    //return 0.1 * tanh(10.0 * z);
  };
 
  /// squashing function with adjustable clipping size, to protect against too large weight updates
  static double squash(void* d, double z) {
    double size = *((double*)d);
    return z < -size ? -size : ( z > size ? size : z );
  };
 
};

#endif

---