invertmotornstep.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                                        *
 *    joergweide84@aol.com (robot12)                                       *
 *                                                                         *
 *   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: invertmotornstep.h,v $
 *   Revision 1.37  2009/10/23 12:39:03  martius
 *   added description for inspectable elements
 *
 *   Revision 1.36  2009/08/07 14:40:23  martius
 *   added new teaching interface
 *
 *   Revision 1.35  2009/08/04 17:23:08  martius
 *   removed additional constructor again and added a initial matrix into the conf
 *
 *   Revision 1.34  2009/07/15 13:10:11  robot12
 *   insert a new constructor to create the class with a giving neuron matrix
 *
 *   Revision 1.33  2009/03/20 17:52:33  martius
 *   modified teaching
 *
 *   Revision 1.32  2008/05/30 11:58:27  martius
 *   use cmath instead of math.h
 *
 *   Revision 1.31  2008/04/17 14:54:44  martius
 *   randomGen added, which is a random generator with long period and an
 *    internal state. Each Agent has an instance and passed it to the controller
 *    and the wiring. This is good for
 *   a) repeatability on agent basis,
 *   b) parallel execution as done in ode_robots
 *
 *   Revision 1.30  2008/01/17 10:02:00  der
 *   inserted some parameters.
 *
 *   Revision 1.29  2007/12/13 16:42:52  martius
 *   continuity
 *
 *   Revision 1.28  2007/12/07 10:58:02  der
 *   NoiseGenerator: method signature of add and generate changed!
 *   IMNS: added cfactor parameter
 *
 *   Revision 1.27  2007/11/07 13:36:36  martius
 *   context sensor definition, that are inputs to S
 *
 *   Revision 1.26  2007/04/02 08:21:36  martius
 *   simple reinforcement
 *
 *   Revision 1.25  2007/02/23 19:36:17  martius
 *   S and SD matrixes separated
 *
 *   Revision 1.24  2007/02/12 13:18:46  martius
 *   teaching changed. Use standard backprop
 *   distal learning
 *   model compliant learning (untested)
 *
 *   Revision 1.23  2007/01/26 12:03:11  martius
 *   kwta inhibition and limitRF implemented
 *
 *   Revision 1.22  2006/12/21 11:44:17  martius
 *   commenting style for doxygen //< -> ///<
 *   FOREACH and FOREACHC are macros for collection iteration
 *
 *   Revision 1.21  2006/12/01 16:18:26  martius
 *   S uses first and second derivative. Not really tested yet
 *
 *   Revision 1.20  2006/11/29 16:22:43  martius
 *   name is a variable of configurable and is used as such
 *
 *   Revision 1.19  2006/10/23 10:47:30  martius
 *   model as function
 *
 *   Revision 1.18  2006/08/02 09:31:16  martius
 *   calcErrorFactor moved to invertmotorcontroller
 *
 *   Revision 1.17  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.16  2006/07/18 15:17:16  martius
 *   buffer operations like delayed values and smoothed values moved to invertmotorcontroller
 *
 *   Revision 1.15  2006/07/18 14:47:45  martius
 *   new regularisation of g' and g''/g' and squashP moved to invertmotorcontroller
 *
 *   Revision 1.14  2006/07/14 12:23:58  martius
 *   selforg becomes HEAD
 *
 *   Revision 1.12.6.12  2006/07/10 13:05:16  martius
 *   NON-COMMERICAL LICENSE added to controllers
 *
 *   Revision 1.12.6.11  2006/07/10 11:59:23  martius
 *   Matrixlib now in selforg
 *   no namespace std in header files
 *
 *   Revision 1.12.6.10  2006/02/24 14:46:44  martius
 *   removed gR
 *
 *   Revision 1.12.6.9  2006/02/08 15:16:24  martius
 *   *** empty log message ***
 *
 *   Revision 1.12.6.8  2006/01/31 15:30:34  martius
 *   model does not learn if too large error.
 *
 *   Revision 1.12.6.7  2006/01/18 16:49:34  martius
 *   new Update algo (simpler and hopefully better)
 *
 *   Revision 1.12.6.6  2006/01/17 16:58:40  martius
 *   loading and storing
 *
 *   Revision 1.12.6.5  2006/01/13 12:24:39  martius
 *   motor teaching included
 *
 *   Revision 1.12.6.4  2005/12/15 17:04:39  martius
 *   min, max and so on are template functions now
 *
 *   Revision 1.12.6.3  2005/11/21 17:02:46  martius
 *   calcCandHUpdates as separat function
 *   better formula for g' (q') using Mittelwertsatz
 *    works great!
 *
 *   Revision 1.12.6.2  2005/11/16 11:24:26  martius
 *   moved to selforg
 *
 *   Revision 1.12.6.1  2005/11/14 14:54:02  martius
 *   cpp file is seperate now
 *
 *   Revision 1.12  2005/10/27 15:46:38  martius
 *   inspectable interface is expanded to structural information for network visualiser
 *
 *   Revision 1.11  2005/10/24 11:10:27  martius
 *   disabled exact formula for shift
 *
 *   Revision 1.10  2005/10/21 16:02:49  martius
 *   added additional model matrix S (switch in contructor)
 *
 *   Revision 1.9  2005/10/21 11:55:13  martius
 *   id is now toId
 *   relativeE can be switched on
 *   uses an symmetric but exact formula for shift calcuation through neuron
 *   However g'/g'' stays -2g because implementation was making trouble
 *
 *   Revision 1.8  2005/10/06 17:08:08  martius
 *   switched to stl lists
 *   Bias noise
 *   correct error_factor for controller learning
 *
 *   Revision 1.7  2005/09/27 10:59:47  martius
 *   error_factor (from logaE or rootE) is member now
 *
 *   Revision 1.6  2005/09/22 07:22:59  martius
 *   proper destruction
 *
 *   Revision 1.5  2005/09/21 07:31:10  martius
 *   v does not go through g
 *
 *   Revision 1.4  2005/07/28 11:16:08  martius
 *   there is a switch for exporting just some of the internal parameters
 *
 *   Revision 1.3  2005/07/27 15:44:23  martius
 *   learning rate adaptation
 *
 *   Revision 1.2  2005/07/21 15:07:59  martius
 *   working version
 *   squashing normalised
 *
 *   Revision 1.1  2005/07/19 09:35:35  martius
 *   invert motor space controller that iterates
 *    over multiple time steps in past
 *
 *
 ***************************************************************************/
#ifndef __INVERTMOTORNSTEP_H
#define __INVERTMOTORNSTEP_H

#include "invertmotorcontroller.h"
#include "teachable.h"

#include <assert.h>
#include <cmath>

#include <selforg/matrix.h>
#include <selforg/noisegenerator.h>

typedef struct InvertMotorNStepConf {
  int buffersize; ///< buffersize size of the time-buffer for x,y,eta
  matrix::Matrix initialC; ///< initial controller matrix C (if not given then randomly chosen, see cInit, cNonDiag...)
  double cInit; ///< cInit size of the C matrix to initialised with.
  double cNonDiag; ///< cNonDiag is the size of the nondiagonal elements in respect to the diagonal (cInit) ones
  double cNonDiagAbs; ///< cNonDiag is the value of the nondiagonal elements
  bool useS;    ///< useS decides whether to use the S matrix in addition to the A matrix (sees sensors)
  /** useSD decides whether to use the SD matrix in addition to the A
      matrix (sees first and second derivatives)  */
  bool useSD;
  /** number of context sensors(considered at the end of the sensor
      vector. If not 0, then S will only get them as input  */
  int numberContext;
  bool someInternalParams;  ///< someInternalParams if true only some internal parameters are exported, all otherwise
} InvertMotorNStepConf;

/**
 * class for robot controller that uses the georg's matrixlib for
 *  direct matrix inversion for n channels
 * (simple one layer networks)
 *
 * Implements standart parameters: eps, rho, mu, stepnumber4avg, stepnumber4delay
 */
class InvertMotorNStep : public InvertMotorController, public Teachable {

public:
  InvertMotorNStep(const InvertMotorNStepConf& conf = getDefaultConf());

  virtual void init(int sensornumber, int motornumber, RandGen* randGen = 0);

  virtual ~InvertMotorNStep();

  /// returns the number of sensors the controller was initialised with or 0 if not initialised
  virtual int getSensorNumber() const { return number_sensors; }
  /// returns the mumber of motors the controller was initialised with or 0 if not initialised
  virtual int getMotorNumber() const  { return number_motors; }

  /// performs one step (includes learning).
  /// Calulates motor commands from sensor inputs.
  virtual void step(const sensor* , int number_sensors, motor* , int number_motors);

  /// performs one step without learning. Calulates motor commands from sensor inputs.
  virtual void stepNoLearning(const sensor* , int number_sensors,
                              motor* , int number_motors);

  /**** STOREABLE ****/
  /** stores the controller values to a given file. */
  virtual bool store(FILE* f) const;
  /** loads the controller values from a given file. */
  virtual bool restore(FILE* f);

  /**** INSPECTABLE ****/
  virtual std::list<ILayer> getStructuralLayers() const;
  virtual std::list<IConnection> getStructuralConnections() const;

  /**** TEACHING ****/
  /** The given motor teaching signal is used for this timestep.
      It is used as a feed forward teaching signal for the controller.
      Please note, that the teaching signal has to be given each timestep
       for a continuous teaching process.
   */
  virtual void setMotorTeachingSignal(const motor* teaching, int len);

  /** The given sensor teaching signal (distal learning) is used for this timestep.
      First the belonging motor teachung signal is calculated by the inverse model.
      See setMotorTeachingSignal
   */
  virtual void setSensorTeachingSignal(const sensor* teaching, int len);
  void getLastMotors(motor* motors, int len);
  void getLastSensors(sensor* sensors, int len);


  /**** New TEACHING interface ****/
  /** The given motor teaching signal is used for this timestep. 
      It is used as a feed forward teaching signal for the controller.
      Please note, that the teaching signal has to be given each timestep 
       for a continuous teaching process.
     @param teaching: matrix with dimensions (motornumber,1)
   */
  virtual void setMotorTeaching(const matrix::Matrix& teaching);

  /** The given sensor teaching signal (distal learning) is used for this timestep. 
      The belonging motor teachung signal is calculated by the inverse model.
      See setMotorTeaching
     @param teaching: matrix with dimensions (motorsensors,1)
   */
  virtual void setSensorTeaching(const matrix::Matrix& teaching);  
  /// returns the last motor values (useful for cross motor coupling)
  virtual matrix::Matrix getLastMotorValues();
  /// returns the last sensor values (useful for cross sensor coupling)
  virtual matrix::Matrix getLastSensorValues();

  

  // UNUSED! OLD IMPLEMENTATION which hat some consistency arguments
  void calcCandHUpdatesTeaching(matrix::Matrix& C_update, matrix::Matrix& H_update, int y_delay);

  /**** REINFORCEMENT ****/
  /** set the reinforcement signal for this timestep.
      It is used to calculate a factor for the update.
      Factor = 1-0.95*reinforcement.
      @param reinforcement value between -1 and 1 (-1 bad, 0 neutral, 1 good)
   */
  virtual void setReinforcement(double reinforcement);



  static InvertMotorNStepConf getDefaultConf(){
    InvertMotorNStepConf c;
    c.buffersize = 50;
    c.cInit = 1.0;
    c.cNonDiag = 0;
    c.useS  = false;
    c.useSD  = false;
    c.numberContext  = 0;
    c.someInternalParams = true;
    c.cNonDiagAbs=0.0;
    return c;
  }

  /// sets the sensor channel weights (matrix should be (getSensorNumber() x 1)
  void setSensorWeights(const matrix::Matrix& weights);
  matrix::Matrix getSensorWeights() const {return sensorweights; }
  /// reference to C-matrix
  matrix::Matrix& getC(){return C;};

  double getE() const {return v.multTM().val(0,0);}

protected:
  unsigned short number_sensors;
  unsigned short number_motors;

 public:
  matrix::Matrix A; ///< Model Matrix (motors to sensors)
  matrix::Matrix S; ///< additional Model Matrix (sensors to sensors)
  matrix::Matrix SD; ///< additional Model Matrix (sensors derivatives to sensors)
  matrix::Matrix C; ///< Controller Matrix
  matrix::Matrix H; ///< Controller Bias
  matrix::Matrix B; ///< Model Bias
protected:
  NoiseGenerator* BNoiseGen; ///< Noisegenerator for noisy bias
  NoiseGenerator* YNoiseGen; ///< Noisegenerator for noisy motor output
  matrix::Matrix R; ///< C*A
  matrix::Matrix SmallID; ///< small identity matrix in the dimension of R
  matrix::Matrix xsi; ///< current output error
  matrix::Matrix v;   ///< current reconstructed error
  double E_val; ///< value of Error function
  double xsi_norm; ///< norm of matrix
  double xsi_norm_avg; ///< average norm of xsi (used to define whether Modell learns)
  double pain;         ///< if the modelling error (xsi) is too high we have a pain signal
  matrix::Matrix* x_buffer;
  matrix::Matrix* y_buffer;
  matrix::Matrix* eta_buffer;
  matrix::Matrix zero_eta; // zero initialised eta
  matrix::Matrix x_smooth;
  //   matrix::Matrix z; ///< membrane potential
  matrix::Matrix y_teaching; ///< teaching motor signal
  bool useTeaching; ///< flag whether there is an actual teachning signal or not
  double reinforcement; ///< reinforcement value (set via setReinforcement())
  double reinforcefactor; ///< reinforcement factor (set to 1 every step after learning)
  int t_rand; ///< initial random time to avoid syncronous management of all controllers

  matrix::Matrix sensorweights; ///< sensor channel weight (each channel gets a certain importance)

  /** factor to teach for continuity: subsequent motor commands
      should not differ too much */
  double continuity;
  double modelCompliant; ///< learning factor for model (or sensor) compliant learning
  int managementInterval; ///< interval between subsequent management function calls
  paramval inhibition; ///< inhibition strength for sparce kwta strategy (is scaled with epsC)
  paramval kwta;       ///< (int) number of synapses that get strengthend
  paramval limitRF;    ///< (int) receptive field of motor neurons (number of offcenter sensors) if null then no limitation. Mutual exclusive with inhibition
  paramval dampS;     ///< damping of S matrix
  paramval dampC;     ///< damping of C matrix
  paramval activeExplore; ///< decides whether and how strong the backpropagated error is used as a control signal
  paramval cfactor;
  paramval cnondiagabs;
  paramval cdiagabs;


  paramval noiseY; ///< noise strength for y

  InvertMotorNStepConf conf;

  /// puts the sensors in the ringbuffer, generate controller values and put them in the
  //  ringbuffer as well
  virtual void fillBuffersAndControl(const sensor* x_, int number_sensors,
                             motor* y_, int number_motors);

  /// calculates the first shift into the motor space useing delayed motor values.
  //  @param delay 0 for no delay and n>0 for n timesteps delay in the time loop
  virtual void calcEtaAndBufferIt(int delay);
  /// calculates xsi for the current time step using the delayed y values
  //  and x delayed by one
  //  @param delay 0 for no delay and n>0 for n timesteps delay in the time loop
  virtual void calcXsi(int delay);

  /// learn H,C with motors y and corresponding sensors x
  //  @param delay 0 for no delay and n>0 for n timesteps delay in the time loop
  virtual void learnController(int delay);

  /// calculates the Update for C and H
  // @param delay timesteps to delay the y-values.  (usually 0)
  //  Please note that the delayed values are NOT used for the error calculation
  //  (this is done in calcXsi())
  virtual void calcCandHUpdates(matrix::Matrix& C_update, matrix::Matrix& H_update, int delay);

  /// updates the matrix C and H
  virtual void updateCandH(const matrix::Matrix& C_update, const matrix::Matrix& H_update, double squashSize);

  /// learn A, (and S) using motors y and corresponding sensors x
  //  @param delay 0 for no delay and n>0 for n timesteps delay in the time loop
  virtual void learnModel(int delay);

  /// calculates the predicted sensor values
  virtual matrix::Matrix model(const matrix::Matrix* x_buffer, int delay, const matrix::Matrix& y);

  /// handles inhibition damping etc.
  virtual void management();

  /// returns controller output for given sensor values
  virtual matrix::Matrix calculateControllerValues(const matrix::Matrix& x_smooth);

  /** Calculates first and second derivative and returns both in on matrix (above).
      We use simple discrete approximations:
      \f[ f'(x) = (f(x) - f(x-1)) / 2 \f]
      \f[ f''(x) = f(x) - 2f(x-1) + f(x-2) \f]
      where we have to go into the past because we do not have f(x+1). The scaling can be neglegted.
  */
  matrix::Matrix calcDerivatives(const matrix::Matrix* buffer, int delay);

public:
  /** k-winner take all inhibition for synapses. k largest synapses are strengthed and the rest are inhibited.
      strong synapes are scaled by 1+(damping/k) and weak synapses are scaled by 1-(damping/(n-k)) where n is the
      number of synapes
      @param weightmatrix reference to weight matrix. Synapses for a neuron are in one row.
             The inhibition is done for all rows independently
      @param k number of synapes to strengthen
      @param damping strength of supression and exitation (typically 0.001)
   */
  void kwtaInhibition(matrix::Matrix& weightmatrix, unsigned int k, double damping);

  /** sets all connections to zero which are further away then rfSize
      from the diagonal.
      If rfSize == 1 then only main diagonal is left.
      If rfSize = 2: main diagonal and upper and lower side diagonal are kept and so on and so forth.
   */
  void limitC(matrix::Matrix& weightmatrix, unsigned int rfSize);


  static double clip095(double x);
  static double regularizedInverse(double v);

};

#endif

---