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

# derivativewiring.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 LpzRobots development team                    *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
    00005  *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
    00006  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024 #ifndef __DERIVATIVEWIRING_H
    00025 #define __DERIVATIVEWIRING_H
    00026
    00027 #include "abstractwiring.h"
    00028
    00029 /**  Configuration object for DerivativeWiring.
    00030      If all boolean parametes are false, id is set to true (equivalent to One2OneWiring)
    00031 */
    00032 typedef struct __DerivativeWiringConf {
    00033   bool useId;        //< include zeroth derivative
    00034   bool useFirstD;   ///< include first derivative
    00035   bool useSecondD;  ///< second include second derivative
    00036   double eps;       ///< update rate for floating average (0 -> no sensor variation, 1 -> no smoothing)
    00037   double derivativeScale;   ///< factor for the derivatives
    00038   unsigned int blindMotors;   ///< number of motors that are blind (not given to robot)
    00039 } DerivativeWiringConf;
    00040
    00041
    00042 /** Implements a wiring (between controller and robot)
    00043     which includes the first and second derivative
    00044     of the original robot sensor values
    00045 */
    00046 class DerivativeWiring : public AbstractWiring{
    00047 public:
    00048   /** constructor
    00049       @param conf  for giving the wished configuration of DerivativeWiring
    00050       via \ref __DerivativeWiringConf "DerivativeWiringConf"
    00051       @param noise NoiseGenerator that is used for adding noise to sensor values
    00052   */
    00053   DerivativeWiring(const DerivativeWiringConf& conf,
    00054                    NoiseGenerator* noise, const std::string& name = "DerivativeWiring");
    00055
    00056   /** destructor
    00057    */
    00058   virtual ~DerivativeWiring();
    00059
    00060   /** Providing default configuration for DerivativeWiring with first derivative.
    00061       No smoothing and no scaling. ( as static method )
    00062    */
    00063   static DerivativeWiringConf getDefaultConf(){
    00064     DerivativeWiringConf c;
    00065     c.useId = true;        // use id
    00066     c.useFirstD = false;    // use first derivative
    00067     c.useSecondD = false;  // do not use secound derivative
    00068     c.eps = 1;             // no smoothing
    00069     c.derivativeScale=1;   // no scaleing
    00070     c.blindMotors=0;       // no blind motors used
    00071     return c;
    00072   };
    00073
    00074   /** Providing default configuration for DerivativeWiring for only first derivative.
    00075       smoothing over 4 steps and scale of 5. Use smaller noise!
    00076       ( as static method )
    00077    */
    00078   static DerivativeWiringConf getDefaultConf1(){
    00079     DerivativeWiringConf c;
    00080     c.useId = false;       // do not use id
    00081     c.useFirstD = true;    // use first derivative
    00082     c.useSecondD = false;  // do not use secound derivative
    00083     c.eps = 0.5;          // smoothing over 2 steps
    00084     c.derivativeScale=5;   // scaling with 5
    00085     c.blindMotors=0;       // no blind motors used
    00086     return c;
    00087   };
    00088
    00089   virtual void reset();
    00090
    00091 protected:
    00092
    00093   virtual bool initIntern();
    00094
    00095   virtual bool wireSensorsIntern(const sensor* rsensors, int rsensornumber,
    00096                                  sensor* csensors, int csensornumber,
    00097                                  double noise);
    00098
    00099   virtual bool wireMotorsIntern(motor* rmotors, int rmotornumber,
    00100                                 const motor* cmotors, int cmotornumber);
    00101
    00102 protected:
    00103   /** Calculate the first derivative of the sensorvalues given by the robot
    00104    *  f'(x) = (f(x+1) - f(x-1)) / 2
    00105    *  since we do not have f(x+1) we go one timestep in the past
    00106    */
    00107   void calcFirstDerivative();
    00108
    00109   /** Calculate the secound derivative of the sensorvalues given by the robot
    00110    *  f'(x) = f(x) - 2f(x-1) + f(x-2)
    00111    */
    00112   void calcSecondDerivative();
    00113
    00114   /// used configuration
    00115   DerivativeWiringConf conf;
    00116   static const int buffersize=40;
    00117   int time;
    00118   /// number timesteps the sensor values are delayed for calculation of the derivative
    00119   //  int delay;
    00120
    00121
    00122   /// current and old smoothed sensor values of robot
    00123   sensor* sensorbuffer[buffersize];
    00124
    00125   /// current sensors (with noise)
    00126   // sensor* id;
    00127
    00128   /// current first derivative
    00129   sensor* first;
    00130
    00131   /// current second derivative
    00132   sensor* second;
    00133
    00134   /// array that stored the values of the blind motors
    00135   motor *blindMotors;
    00136
    00137 };
    00138
    00139 #endif