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

# sox.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 __SOX_H
    00020 #define __SOX_H
    00021
    00022 #include <selforg/abstractcontroller.h>
    00023 #include <selforg/controller_misc.h>
    00024
    00025 #include <assert.h>
    00026 #include <cmath>
    00027
    00028 #include <selforg/matrix.h>
    00029 #include <selforg/teachable.h>
    00030
    00031
    00032 /// configuration object for Sox controller. Use Sox::getDefaultConf().
    00033 struct SoxConf {
    00034   double initFeedbackStrength;  ///< initial strength of sensor to motor connection
    00035   bool   useExtendedModel;      ///< if true, the extended model (S matrix) is used
    00036   /// if true the controller can be taught see teachable interface
    00037   bool   useTeaching;
    00038   /// # of steps the sensors are averaged (1 means no averaging)
    00039   int    steps4Averaging;
    00040   /// # of steps the motor values are delayed (1 means no delay)
    00041   int    steps4Delay;
    00042   bool   someInternalParams;    ///< if true only some internal parameters are exported
    00043   bool   onlyMainParameters;    ///< if true only some configurable parameters are exported
    00044
    00045   double factorS;             ///< factor for learning rate of S
    00046   double factorb;             ///< factor for learning rate of b
    00047 };
    00048
    00049
    00050 /**
    00051  * This controller implements the standard algorihm described the the Chapter 5 (Homeokinesis)
    00052  *  with extensions of Chapter 15 of book "The Playful Machine"
    00053  */
    00054 class Sox : public AbstractController, public Teachable {
    00055
    00056 public:
    00057   /// constructor
    00058   Sox(const SoxConf& conf = getDefaultConf());
    00059
    00060   /// constructor provided for convenience, use conf object to customize more
    00061   Sox(double init_feedback_strength, bool useExtendedModel = true,
    00062       bool useTeaching = false );
    00063
    00064   virtual void init(int sensornumber, int motornumber, RandGen* randGen = 0);
    00065
    00066   virtual ~Sox();
    00067
    00068   static SoxConf getDefaultConf(){
    00069     SoxConf conf;
    00070     conf.initFeedbackStrength = 1.0;
    00071     conf.useExtendedModel     = true;
    00072     conf.useTeaching          = false;
    00073     conf.steps4Averaging      = 1;
    00074     conf.steps4Delay          = 1;
    00075     conf.someInternalParams   = false;
    00076     conf.onlyMainParameters   = true;
    00077
    00078     conf.factorS              = 1;
    00079     conf.factorb              = 1;
    00080     return conf;
    00081   }
    00082
    00083
    00084   /// returns the number of sensors the controller was initialised with or 0 if not initialised
    00085   virtual int getSensorNumber() const { return number_sensors; }
    00086   /// returns the mumber of motors the controller was initialised with or 0 if not initialised
    00087   virtual int getMotorNumber() const  { return number_motors; }
    00088
    00089   /// performs one step (includes learning).
    00090   /// Calulates motor commands from sensor inputs.
    00091   virtual void step(const sensor* , int number_sensors, motor* , int number_motors);
    00092
    00093
    00094   /// performs one step without learning. Calulates motor commands from sensor inputs.
    00095   virtual void stepNoLearning(const sensor* , int number_sensors,
    00096                               motor* , int number_motors);
    00097
    00098   /// called during babbling phase
    00099   virtual void motorBabblingStep(const sensor* , int number_sensors,
    00100                                  const motor* , int number_motors);
    00101
    00102   /***** STOREABLE ****/
    00103   /** stores the controller values to a given file. */
    00104   virtual bool store(FILE* f) const;
    00105   /** loads the controller values from a given file. */
    00106   virtual bool restore(FILE* f);
    00107
    00108   /* some direct access functions (unsafe!) */
    00109   virtual matrix::Matrix getA();
    00110   virtual void setA(const matrix::Matrix& A);
    00111   virtual matrix::Matrix getC();
    00112   virtual void setC(const matrix::Matrix& C);
    00113   virtual matrix::Matrix geth();
    00114   virtual void seth(const matrix::Matrix& h);
    00115
    00116   /***** TEACHABLE ****/
    00117   virtual void setMotorTeaching(const matrix::Matrix& teaching);
    00118   virtual void setSensorTeaching(const matrix::Matrix& teaching);
    00119   virtual matrix::Matrix getLastMotorValues();
    00120   virtual matrix::Matrix getLastSensorValues();
    00121
    00122 protected:
    00123   unsigned short number_sensors;
    00124   unsigned short number_motors;
    00125   static const unsigned short buffersize = 10;
    00126
    00127   matrix::Matrix A; // Model Matrix
    00128   matrix::Matrix C; // Controller Matrix
    00129   matrix::Matrix S; // Model Matrix (sensor branch)
    00130   matrix::Matrix h; // Controller Bias
    00131   matrix::Matrix b; // Model Bias
    00132   matrix::Matrix L; // Jacobi Matrix
    00133   matrix::Matrix R; //
    00134   matrix::Matrix C_native; // Controller Matrix obtained from motor babbling
    00135   matrix::Matrix A_native; // Model Matrix obtained from motor babbling
    00136   matrix::Matrix y_buffer[buffersize]; // buffer needed for delay
    00137   matrix::Matrix x_buffer[buffersize]; // buffer of sensor values
    00138   matrix::Matrix v_avg;
    00139   matrix::Matrix x;        // current sensor value vector
    00140   matrix::Matrix x_smooth; // time average of x values
    00141   int t;
    00142
    00143   bool loga;
    00144
    00145   SoxConf conf; ///< configuration objects
    00146
    00147   bool intern_isTeaching;    // teaching signal available?
    00148   matrix::Matrix y_teaching; // motor teaching  signal
    00149
    00150   paramval creativity;
    00151   paramval sense;
    00152   paramval harmony;
    00153   paramval causeaware;
    00154   paramint pseudo;
    00155   paramval epsC;
    00156   paramval epsA;
    00157   paramval damping;
    00158   paramval gamma;          // teaching strength
    00159
    00160   void constructor();
    00161
    00162   // calculates the pseudo inverse of L in different ways, depending on pseudo
    00163   matrix::Matrix pseudoInvL(const matrix::Matrix& L, const matrix::Matrix& A, const matrix::Matrix& C);
    00164
    00165   /// learn values model and controller (A,b,C,h)
    00166   virtual void learn();
    00167
    00168   /// neuron transfer function
    00169   static double g(double z)
    00170   {
    00171     return tanh(z);
    00172   };
    00173
    00174   /// derivative of g
    00175   static double g_s(double z)
    00176   {
    00177     double k=tanh(z);
    00178     return 1.0 - k*k;
    00179   };
    00180
    00181   /// function that clips the second argument to the interval [-first,first]
    00182   static double clip(double r, double x){
    00183     return min(max(x,-r),r);
    00184   }
    00185   /// calculates the inverse the argument (useful for Matrix::map)
    00186   static double one_over(double x){
    00187     return 1/x;
    00188   }
    00189
    00190
    00191 };
    00192
    00193 #endif
    00194
    00195