Robotsystem of the Robot Group Leipzig: basiccontroller.h Source File

- Main Page
- Namespaces
- Classes
- Files
- Directories
- Related Pages
- Examples

- Search for |

- File List
- File Members

# basiccontroller.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   ANY COMMERCIAL USE FORBIDDEN!                                         *
    00008  *   LICENSE:                                                              *
    00009  *   This work is licensed under the Creative Commons                      *
    00010  *   Attribution-NonCommercial-ShareAlike 2.5 License. To view a copy of   *
    00011  *   this license, visit http://creativecommons.org/licenses/by-nc-sa/2.5/ *
    00012  *   or send a letter to Creative Commons, 543 Howard Street, 5th Floor,   *
    00013  *   San Francisco, California, 94105, USA.                                *
    00014  *                                                                         *
    00015  *   This program is distributed in the hope that it will be useful,       *
    00016  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00017  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.                  *
    00018  *                                                                         *
    00019  *   $Log: basiccontroller.h,v $
    00020  *   Revision 1.3  2008/05/30 11:58:27  martius
    00021  *   use cmath instead of math.h
    00022  *
    00023  *   Revision 1.2  2008/04/17 14:54:44  martius
    00024  *   randomGen added, which is a random generator with long period and an
    00025  *    internal state. Each Agent has an instance and passed it to the controller
    00026  *    and the wiring. This is good for
    00027  *   a) repeatability on agent basis,
    00028  *   b) parallel execution as done in ode_robots
    00029  *
    00030  *   Revision 1.1  2008/02/28 07:45:18  der
    00031  *   Simple version of the controller with regularized inversion in sensor space.
    00032  *
    00033  *   Revision 1.1  2007/12/06 16:50:49  der
    00034  *   new versions
    00035  *
    00036  *
    00037  *
    00038  *
    00039  ***************************************************************************/
    00040 #ifndef __BASICCONTROLLER_H
    00041 #define __BASICCONTROLLER_H
    00042
    00043 #include "invertmotorcontroller.h"
    00044
    00045 #include <assert.h>
    00046 #include <cmath>
    00047
    00048 #include "matrix.h"
    00049 #include "noisegenerator.h"
    00050 #include "invertablemodel.h"
    00051
    00052 typedef struct BasicControllerConf {
    00053   int buffersize;  ///< buffersize size of the time-buffer for x,y,eta
    00054   double cInit;    ///< cInit size of the C matrix to initialised with.
    00055   double cNonDiag; ///< cNonDiag is the size of the nondiagonal elements in respect to the diagonal (cInit) ones
    00056   bool modelInit;  ///< size of the unit-map strenght of the model
    00057   bool useS;    ///< useS decides whether to use the S matrix in addition to the A matrix
    00058   bool someInternalParams;  ///< someInternalParams if true only some internal parameters are exported, otherwise all
    00059
    00060   double modelCompliant; ///< learning factor for model (or sensor) compliant learning
    00061   bool useFantasy;           ///< if true fantasising is enabled
    00062
    00063   InvertableModel* model;   ///< model used as world model
    00064 } BasicControllerConf;
    00065
    00066 /**
    00067  * class for robot controller is based on InvertMotorNStep
    00068  *
    00069  * - direct inversion
    00070  *
    00071  * - motor space
    00072  *
    00073  * - multilayer,nonlinear model
    00074  */
    00075 class BasicController : public InvertMotorController {
    00076
    00077 public:
    00078   BasicController(const BasicControllerConf& conf = getDefaultConf());
    00079   virtual void init(int sensornumber, int motornumber, RandGen* randGen = 0);
    00080
    00081   virtual ~BasicController();
    00082
    00083   /// returns the number of sensors the controller was initialised with or 0 if not initialised
    00084   virtual int getSensorNumber() const { return number_sensors; }
    00085   /// returns the mumber of motors the controller was initialised with or 0 if not initialised
    00086   virtual int getMotorNumber() const  { return number_motors; }
    00087
    00088   /// performs one step (includes learning).
    00089   /// Calulates motor commands from sensor inputs.
    00090   virtual void step(const sensor* , int number_sensors, motor* , int number_motors);
    00091
    00092   /// performs one step without learning. Calulates motor commands from sensor inputs.
    00093   virtual void stepNoLearning(const sensor* , int number_sensors,
    00094                               motor* , int number_motors);
    00095
    00096
    00097   /**************  STOREABLE **********************************/
    00098   /** stores the controller values to a given file. */
    00099   virtual bool store(FILE* f) const;
    00100   /** loads the controller values from a given file. */
    00101   virtual bool restore(FILE* f);
    00102
    00103   /************** INSPECTABLE ********************************/
    00104   virtual iparamkeylist getInternalParamNames() const;
    00105   virtual iparamvallist getInternalParams() const;
    00106   virtual ilayerlist getStructuralLayers() const;
    00107   virtual iconnectionlist getStructuralConnections() const;
    00108
    00109   /************** CONFIGURABLE ********************************/
    00110   virtual bool setParam(const paramkey& key, paramval val);
    00111
    00112   /**** TEACHING ****/
    00113   /** The given motor teaching signal is used for this timestep.
    00114       It is used as a feed forward teaching signal for the controller.
    00115       Please note, that the teaching signal has to be given each timestep
    00116        for a continuous teaching process.
    00117    */
    00118   //  virtual void setMotorTeachingSignal(const motor* teaching, int len);
    00119
    00120   /** The given sensor teaching signal (distal learning) is used for this timestep.
    00121       First the belonging motor teachung signal is calculated by the inverse model.
    00122       See setMotorTeachingSignal
    00123    */
    00124   //  virtual void setSensorTeachingSignal(const sensor* teaching, int len);
    00125
    00126
    00127   static BasicControllerConf getDefaultConf(){
    00128     BasicControllerConf c;
    00129     c.buffersize = 50;
    00130     c.cInit = 1.05;
    00131     c.cNonDiag = 0;
    00132     c.modelInit  = 1.0;
    00133      c.someInternalParams = true;
    00134      //   c.someInternalParams = false;
    00135     c.useS = false;
    00136     c.modelCompliant = 0;
    00137     c.model = 0;
    00138     c.useFantasy = false;
    00139     return c;
    00140   }
    00141
    00142   void getLastMotors(motor* motors, int len);
    00143
    00144 protected:
    00145   unsigned short number_sensors;
    00146   unsigned short number_motors;
    00147
    00148   matrix::Matrix A; ///< Model Matrix (motors to sensors)
    00149   matrix::Matrix A_Hat; ///< Model Matrix (motors to sensors) with input shift
    00150   matrix::Matrix S; ///< additional Model Matrix (sensors to sensors)
    00151   matrix::Matrix C; ///< Controller Matrix
    00152   matrix::Matrix GSC; ///< G_Prime times Controller Matrix
    00153   matrix::Matrix DD; ///< Noise  Matrix
    00154   matrix::Matrix Dinverse; ///< Inverse  Noise  Matrix
    00155   matrix::Matrix H; ///< Controller Bias
    00156   matrix::Matrix B; ///< Model Bias
    00157   NoiseGenerator* BNoiseGen; ///< Noisegenerator for noisy bias
    00158   NoiseGenerator* YNoiseGen; ///< Noisegenerator for noisy motor values
    00159   matrix::Matrix R; ///< C*A
    00160   matrix::Matrix RRT_inv; // (R*R^T)^-1
    00161   matrix::Matrix ATA_inv; // ((A^T)*A)^-1
    00162   matrix::Matrix Rm1; ///< R^-1
    00163   matrix::Matrix ID; ///< identity matrix in the dimension of R
    00164   matrix::Matrix ID_Sensor; ///< identity matrix in the dimension of sensor space
    00165
    00166   matrix::Matrix CST;
    00167   matrix::Matrix CCT_inv;
    00168   matrix::Matrix xsi; ///< current output error
    00169   double xsi_norm; ///< norm of matrix
    00170   double xsi_norm_avg; ///< average norm of xsi (used to define whether Modell learns)
    00171   double pain;         ///< if the modelling error (xsi) is too high we have a pain signal
    00172   matrix::Matrix* x_buffer;
    00173   matrix::Matrix* y_buffer;
    00174   matrix::Matrix* eta_buffer;
    00175   matrix::Matrix eta;
    00176   matrix::Matrix v_smooth;
    00177   matrix::Matrix zero_eta; // zero initialised eta
    00178   matrix::Matrix x_smooth;
    00179   matrix::Matrix y_smooth;
    00180   matrix::Matrix eta_smooth;
    00181   matrix::Matrix x_smooth_long;
    00182
    00183   matrix::Matrix y_teaching; ///< teaching motor signal
    00184   bool useTeaching; ///< flag whether there is an actual teachning signal or not
    00185
    00186   matrix::Matrix x_intern;  ///< fantasy sensor values
    00187   int fantControl;     ///< interval length for fantasising
    00188   int fantControlLen;  ///< length of fantasy control
    00189   int fantReset;       ///< number of fantasy control events before reseting internal state
    00190
    00191   int t_rand; ///< initial random time to avoid syncronous management of all controllers
    00192   int managementInterval; ///< interval between subsequent management function calls
    00193   paramval dampS;     ///< damping of S matrix
    00194   paramval dampC;     ///< damping of C matrix
    00195   paramval dampH;     ///< damping of H vector
    00196   paramval weighting;     ///< general weighting fator between update concepts
    00197
    00198   BasicControllerConf conf;
    00199
    00200   /// puts the sensors in the ringbuffer, generate controller values and put them in the
    00201   //  ringbuffer as well
    00202   virtual void fillBuffersAndControl(const sensor* x_, int number_sensors,
    00203                              motor* y_, int number_motors);
    00204
    00205 /** learn values H,C
    00206     This is the implementation uses a better formula for g^-1 using Mittelwertsatz
    00207     @param delay 0 for no delay and n>0 for n timesteps delay in the SML (s4delay)
    00208 */
    00209   virtual void learnController(int delay);
    00210
    00211   /// learn conf.model, (and S) using motors y and corresponding sensors x
    00212   //  @param delay 0 for no delay and n>0 for n timesteps delay in the time loop
    00213   virtual void learnModel(int delay);
    00214
    00215   /// handles inhibition damping etc.
    00216   //  virtual void management();
    00217
    00218   /// returns controller output for given sensor values
    00219   virtual matrix::Matrix calculateControllerValues(const matrix::Matrix& x_smooth);
    00220
    00221   /** Calculates first and second derivative and returns both in on matrix (above).
    00222       We use simple discrete approximations:
    00223       \f[ f'(x) = (f(x) - f(x-1)) / 2 \f]
    00224       \f[ f''(x) = f(x) - 2f(x-1) + f(x-2) \f]
    00225       where we have to go into the past because we do not have f(x+1). The scaling can be neglegted.
    00226   */
    00227   matrix::Matrix calcDerivatives(const matrix::Matrix* buffer, int delay);
    00228
    00229 public:
    00230
    00231   /// calculates the city block distance (abs) norm of the matrix. (abs sum of absolutes / size of matrix)
    00232      virtual double calcMatrixNorm(const matrix::Matrix& m);
    00233
    00234 };
    00235
    00236 #endif