Robot Simulator of the Robotics Group for Self-Organization of Control: semox.h Source File

# semox.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 by                                            *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00005  *                                                                         *
    00006  *   ANY COMMERCIAL USE FORBIDDEN!                                         *
    00007  *   LICENSE:                                                              *
    00008  *   This work is licensed under the Creative Commons                      *
    00009  *   Attribution-NonCommercial-ShareAlike 2.5 License. To view a copy of   *
    00010  *   this license, visit http://creativecommons.org/licenses/by-nc-sa/2.5/ *
    00011  *   or send a letter to Creative Commons, 543 Howard Street, 5th Floor,   *
    00012  *   San Francisco, California, 94105, USA.                                *
    00013  *                                                                         *
    00014  *   This program is distributed in the hope that it will be useful,       *
    00015  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00016  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.                  *
    00017  *                                                                         *
    00018  ***************************************************************************/
    00019 #ifndef __SEMOX_H
    00020 #define __SEMOX_H
    00021
    00022 #include <selforg/homeokinbase.h>
    00023 #include <selforg/matrix.h>
    00024 #include <selforg/teachable.h>
    00025 #include <selforg/noisegenerator.h>
    00026 #include <selforg/randomgenerator.h>
    00027
    00028
    00029 typedef struct SeMoXConf {
    00030   int buffersize;   ///< buffersize size of the time-buffer for x,y,eta
    00031   matrix::Matrix initialC; ///< initialC initial controller matrix (if null matrix then automatic, see cInit)
    00032   double cInit;     ///< cInit initial size of the diagonals of the C matrix (if C is not given)
    00033   double cNonDiag;  ///< cNonDiag initial size of the non-diagonal elements of the C matrix (if C is not given)
    00034   double aInit;     ///< aInit initial size of the diagonals of the A matrix
    00035   double sInit;     ///< sInit initial size of the diagonals of the S matrix
    00036   bool modelExt;    ///< modelExt if true then additional matrix S is used in forward model (sees sensors)
    00037   /** number of context sensors (considered at the end of the sensor
    00038       vector, which are only feed to the model extended model */
    00039   int numContext;
    00040   bool someInternalParams;  ///< someInternalParams if true only some internal parameters are exported
    00041 } SeMoXConf;
    00042
    00043 /**
    00044  * This controller follows the prinziple of homeokinesis and
    00045  *  implements the extensions described in the thesis of Georg Martius
    00046  *  2009, University Goettingen:
    00047  *  Goal-Oriented Control of Self-organizing Behavior in Autonomous Robots
    00048  *
    00049  * This class also implements part of the guided self-organization
    00050  *
    00051  * Name: SElf-organizing MOtor space eXtended
    00052  *
    00053  * Main characteristics: Motor Space, Extended World model, Continuity, Teaching interface
    00054  *
    00055  */
    00056 class SeMoX : public HomeokinBase, public Teachable {
    00057   friend class ThisSim;
    00058 public:
    00059   SeMoX(const SeMoXConf& conf = getDefaultConf());
    00060
    00061   /// returns the default configuration
    00062   static SeMoXConf getDefaultConf(){
    00063     SeMoXConf c;
    00064     c.buffersize = 50;
    00065     // c.initialC // remains 0x0
    00066     c.cInit = 1.0;
    00067     c.cNonDiag = 0;
    00068     c.aInit = 1.0;
    00069     c.sInit = 0.0;
    00070     c.modelExt  = true;
    00071     c.someInternalParams = true;
    00072     c.numContext = 0;
    00073     return c;
    00074   }
    00075
    00076   virtual void init(int sensornumber, int motornumber, RandGen* randGen = 0);
    00077
    00078   virtual ~SeMoX();
    00079
    00080   /// returns the number of sensors the controller was initialised with or 0 if not initialised
    00081   virtual int getSensorNumber() const { return number_sensors; }
    00082   /// returns the mumber of motors the controller was initialised with or 0 if not initialised
    00083   virtual int getMotorNumber() const  { return number_motors; }
    00084
    00085   /// performs one step (includes learning).
    00086   /// Calulates motor commands from sensor inputs.
    00087   virtual void step(const sensor* , int number_sensors, motor* , int number_motors);
    00088
    00089   /// performs one step without learning. Calulates motor commands from sensor inputs.
    00090   virtual void stepNoLearning(const sensor* , int number_sensors,
    00091                               motor* , int number_motors);
    00092
    00093   /**** STOREABLE ****/
    00094   /** stores the controller values to a given file. */
    00095   virtual bool store(FILE* f) const;
    00096   /** loads the controller values from a given file. */
    00097   virtual bool restore(FILE* f);
    00098
    00099   /**** INSPECTABLE ****/
    00100   virtual std::list<ILayer> getStructuralLayers() const;
    00101   virtual std::list<IConnection> getStructuralConnections() const;
    00102
    00103   /**** TEACHABLE ****/
    00104   /** The given motor teaching signal is used for this timestep.
    00105       It is used as a feed forward teaching signal for the controller.
    00106       Please note, that the teaching signal has to be given each timestep
    00107        for a continuous teaching process.
    00108      @param teaching: matrix with dimensions (motornumber,1)
    00109    */
    00110   virtual void setMotorTeaching(const matrix::Matrix& teaching);
    00111
    00112   /** The given sensor teaching signal (distal learning) is used for this timestep.
    00113       The belonging motor teachung signal is calculated by the inverse model.
    00114       See setMotorTeaching
    00115      @param teaching: matrix with dimensions (motorsensors,1)
    00116    */
    00117   virtual void setSensorTeaching(const matrix::Matrix& teaching);
    00118   /// returns the last motor values (useful for cross motor coupling)
    00119   virtual matrix::Matrix getLastMotorValues();
    00120   /// returns the last sensor values (useful for cross sensor coupling)
    00121   virtual matrix::Matrix getLastSensorValues();
    00122
    00123 protected:
    00124   unsigned short number_sensors;
    00125   unsigned short number_motors;
    00126
    00127   matrix::Matrix A; ///< Model Matrix (motors to sensors)
    00128   matrix::Matrix S; ///< additional Model Matrix (sensors derivatives to sensors)
    00129   matrix::Matrix C; ///< Controller Matrix
    00130   matrix::Matrix H; ///< Controller Bias
    00131   matrix::Matrix B; ///< Model Bias
    00132   matrix::Matrix R; ///< C*A
    00133   matrix::Matrix SmallID; ///< small identity matrix in the dimension of R
    00134   matrix::Matrix v; ///< shift
    00135   matrix::Matrix xsi; ///< current output error
    00136
    00137   NoiseGenerator* BNoiseGen; ///< Noisegenerator for noisy bias
    00138   paramval modelNoise;       ///< strength of noisy bias
    00139
    00140   double xsi_norm;     ///< norm of matrix
    00141   double xsi_norm_avg; ///< average norm of xsi (used to define whether Modell learns)
    00142   double pain;         ///< if the modelling error (xsi) is too high we have a pain signal
    00143
    00144   matrix::Matrix* x_buffer;
    00145   matrix::Matrix* x_c_buffer; ///< buffer for sensors with context sensors
    00146   matrix::Matrix* y_buffer;
    00147
    00148   matrix::Matrix y_teaching; ///< motor teaching  signal
    00149
    00150   paramval gamma_cont;  ///< parameter to include contiuity in motor values (avoid high frequencies)
    00151   paramval gamma_teach; ///< strength of teaching
    00152   paramval discountS;   ///< discount strength for hierachical model
    00153
    00154   paramval dampModel;       ///< damping of A and S matrices
    00155   paramval dampController;  ///< damping of C matrix
    00156
    00157   SeMoXConf conf;
    00158
    00159   // internal
    00160   bool intern_useTeaching; ///< flag whether there is an actual teachning signal or not
    00161   int t_rand; ///< initial random time to avoid syncronous management of all controllers
    00162   int managementInterval; ///< interval between subsequent management function calls
    00163   parambool _modelExt_copy; ///< copy of modelExtension variable (to achieve readonly)
    00164
    00165   /// puts the sensors in the ringbuffer, generate controller values and put them in the
    00166   //  ringbuffer as well
    00167   virtual void fillBuffersAndControl(const sensor* x_, int number_sensors,
    00168                              motor* y_, int number_motors);
    00169
    00170   /// calculates xsi for the current time step using the delayed y values
    00171   //  and x delayed by one
    00172   //  @param delay 0 for no delay and n>0 for n timesteps delay in the time loop
    00173   virtual void calcXsi(int delay);
    00174
    00175   /// learn H,C with motors y and corresponding sensors x
    00176   virtual void learnController();
    00177
    00178   /// learn A, (and S) using motors y and corresponding sensors x
    00179   //  @param delay 0 for no delay and n>0 for n timesteps delay in the time loop
    00180   virtual void learnModel(int delay);
    00181
    00182   /// calculates the predicted sensor values
    00183   virtual matrix::Matrix model(const matrix::Matrix* x_buffer, int delay, const matrix::Matrix& y);
    00184
    00185   /// handles inhibition damping etc.
    00186   virtual void management();
    00187
    00188   /// returns controller output for given sensor values
    00189   virtual matrix::Matrix calculateControllerValues(const matrix::Matrix& x_smooth);
    00190
    00191 protected:
    00192   static double regularizedInverse(double v);
    00193
    00194
    00195 };
    00196
    00197 #endif