Robotsystem from Robot Group Leipzig: derivativewiring.h Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# derivativewiring.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  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: derivativewiring.h,v $
    00023  *   Revision 1.1.2.1  2005/11/16 11:24:28  martius
    00024  *   moved to selforg
    00025  *
    00026  *   Revision 1.6  2005/10/28 12:05:27  martius
    00027  *   adapted time horizont for derivative
    00028  *    to quater of the time horizont of averaging
    00029  *
    00030  *   Revision 1.5  2005/10/24 13:32:07  fhesse
    00031  *   comments adjusted and in doxygen style
    00032  *
    00033  *   Revision 1.4  2005/10/24 11:06:33  fhesse
    00034  *   comments adjusted and in doxygen style
    00035  *
    00036  *   Revision 1.3  2005/07/21 15:09:00  martius
    00037  *   blind motors
    00038  *
    00039  *   Revision 1.2  2005/07/21 11:30:59  fhesse
    00040  *   started with blind motors
    00041  *
    00042  *   Revision 1.1  2005/07/18 14:44:55  martius
    00043  *   wiring that supports derivatives
    00044  *
    00045  *                                                                         *
    00046  ***************************************************************************/
    00047 #ifndef __DERIVATIVEWIRING_H
    00048 #define __DERIVATIVEWIRING_H
    00049
    00050 #include "abstractwiring.h"
    00051
    00052 /**  Configuration Object for DerivativeWiring.
    00053      If all boolean parametes are false, id is set to true (equivalent to One2OneWiring)
    00054 */
    00055 typedef struct __DerivativeWiringConf {
    00056   bool useId;        //< include zeroth derivative
    00057   bool useFirstD;   //< include first derivative
    00058   bool useSecondD;  //< second include second derivative
    00059   double eps;       //< update rate for floating average (0 -> no sensor variation, 1 -> no smoothing)
    00060   double derivativeScale;   //< factor for the derivatives
    00061   unsigned int blindMotorSets;   //< number of complete motor sets that are blind (not given to robot)
    00062 } DerivativeWiringConf;
    00063
    00064
    00065 /** Implements a wiring (between controller and robot)
    00066     which includes the first and second derivative
    00067     of the original robot sensor values
    00068 */
    00069 class DerivativeWiring : public AbstractWiring{
    00070 public:
    00071   /** constructor
    00072       @param conf  for giving the wished configuration of DerivativeWiring
    00073       via \ref __DerivativeWiringConf "DerivativeWiringConf"
    00074       @param noise NoiseGenerator that is used for adding noise to sensor values
    00075   */
    00076   DerivativeWiring(const DerivativeWiringConf& conf,
    00077                    NoiseGenerator* noise);
    00078
    00079   /** destructor
    00080    */
    00081   virtual ~DerivativeWiring();
    00082
    00083   /** initializes the internal numbers of sensors and motors on robot side, calculate
    00084       number of sensors and motors on controller side
    00085   */
    00086   virtual bool init(int robotsensornumber, int robotmotornumber);
    00087
    00088   /** Realizes derivative wiring from robot sensors to controller sensors.
    00089       @param rsensors pointer to array of sensorvalues from robot
    00090       @param rsensornumber number of sensors from robot
    00091       @param csensors pointer to array of sensorvalues for controller
    00092       @param csensornumber number of sensors to controller
    00093       @param noise size of the noise added to the sensors
    00094   */
    00095   virtual bool wireSensors(const sensor* rsensors, int rsensornumber,
    00096                            sensor* csensors, int csensornumber,
    00097                            double noise);
    00098
    00099   /** Realizes wiring from controller motor outputs to robot motors.
    00100       @param rmotors pointer to array of motorvalues for robot
    00101       @param rmotornumber number of robot motors
    00102       @param cmotors pointer to array of motorvalues from controller
    00103       @param cmotornumber number of motorvalues from controller
    00104   */
    00105   virtual bool wireMotors(motor* rmotors, int rmotornumber,
    00106                           const motor* cmotors, int cmotornumber);
    00107
    00108   /** Providing default configuration for DerivativeWiring as static method
    00109    */
    00110   static DerivativeWiringConf getDefaultConf(){
    00111     DerivativeWiringConf c;
    00112     c.useId = true;        // use id
    00113     c.useFirstD = false;   // do not use first derivative
    00114     c.useSecondD = false;  // do not use secound derivative
    00115     c.eps = 0.05;
    00116     c.derivativeScale=3;
    00117     c.blindMotorSets=0;    // no blind motors used
    00118     return c;
    00119   };
    00120
    00121 protected:
    00122   /** Calculate the first derivative of the sensorvalues given by the robot
    00123    *  f'(x) = (f(x+1) - f(x-1)) / 2
    00124    *  since we do not have f(x+1) we go one timestep in the past
    00125    */
    00126   void calcFirstDerivative();
    00127
    00128   /** Calculate the secound derivative of the sensorvalues given by the robot
    00129    *  f'(x) = f(x) - 2f(x-1) + f(x-2)
    00130    */
    00131   void calcSecondDerivative();
    00132
    00133   /// used configuration
    00134   DerivativeWiringConf conf;
    00135   static const int buffersize=40;
    00136   int time;
    00137   /// number timesteps the sensor values are delayed for calculation of the derivative
    00138   int delay;
    00139
    00140
    00141   /// current and old smoothed sensor values of robot
    00142   sensor* sensorbuffer[buffersize];
    00143
    00144   /// current sensors (with noise)
    00145   sensor* id;
    00146
    00147   /// current first derivative
    00148   sensor* first;
    00149
    00150   /// current second derivative
    00151   sensor* second;
    00152
    00153   /// array that stored the values of the blind motors
    00154   motor *blindMotors;
    00155
    00156   /// number of blind motors used
    00157   unsigned int blindMotorNumber;
    00158 };
    00159
    00160 #endif