Robotsystem from Robot Group Leipzig: derivativewiring.cpp Source File

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

- File List
- File Members

# derivativewiring.cpp

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.cpp,v $
    00023  *   Revision 1.1.2.3  2006/02/08 16:20:21  martius
    00024  *   delay increased by 1
    00025  *
    00026  *   Revision 1.1.2.2  2006/01/30 14:13:56  martius
    00027  *   order has changed from id_1,..,id_n, first_1,...first_n, ...
    00028  *    to id_1,first_1, second_1, id_2, first2, .... id_n, first_n, second_n
    00029  *
    00030  *   Revision 1.1.2.1  2005/11/16 11:24:27  martius
    00031  *   moved to selforg
    00032  *
    00033  *   Revision 1.7  2005/11/14 12:48:08  martius
    00034  *   optimised
    00035  *
    00036  *   Revision 1.6  2005/10/28 12:05:27  martius
    00037  *   adapted time horizont for derivative
    00038  *    to quater of the time horizont of averaging
    00039  *
    00040  *   Revision 1.5  2005/10/24 11:06:33  fhesse
    00041  *   comments adjusted and in doxygen style
    00042  *
    00043  *   Revision 1.4  2005/07/26 17:02:37  martius
    00044  *   2.derivative scaled correctly
    00045  *
    00046  *   Revision 1.3  2005/07/21 15:09:13  martius
    00047  *   blind motors
    00048  *
    00049  *   Revision 1.2  2005/07/21 11:30:59  fhesse
    00050  *   started with blind motors
    00051  *
    00052  *   Revision 1.1  2005/07/18 14:44:55  martius
    00053  *   wiring that supports derivatives
    00054  *
    00055  *                                                                         *
    00056  ***************************************************************************/
    00057
    00058 #include "derivativewiring.h"
    00059
    00060
    00061 /// constructor
    00062 DerivativeWiring::DerivativeWiring(const DerivativeWiringConf& conf,
    00063                                    NoiseGenerator* noise)
    00064   : AbstractWiring::AbstractWiring(noise) {
    00065
    00066   this->conf=conf;
    00067   time     = buffersize;
    00068   delay    = min(buffersize/2-1, int(0.25/(conf.eps+0.01))+1);
    00069   // make sure that at least id is on.
    00070   if (!conf.useFirstD && !conf.useSecondD) this->conf.useId=true;
    00071 }
    00072
    00073 DerivativeWiring::~DerivativeWiring(){
    00074   for(int i=0 ; i<buffersize; i++){
    00075     if(sensorbuffer[i]) free(sensorbuffer[i]);
    00076   }
    00077   if(id) free(id);
    00078   if(first) free(first);
    00079   if(second) free(second);
    00080   if(blindMotors) free(blindMotors);
    00081 }
    00082
    00083
    00084 bool DerivativeWiring::init(int rsensornumber, int rmotornumber){
    00085   this->rsensornumber = rsensornumber;
    00086   this->rmotornumber  = rmotornumber;
    00087   blindMotorNumber = this->rmotornumber * conf.blindMotorSets;
    00088
    00089   this->csensornumber = this->rsensornumber*( (int)conf.useId+(int)conf.useFirstD+(int)conf.useSecondD)
    00090     + blindMotorNumber;
    00091   this->cmotornumber  = this->rmotornumber + blindMotorNumber;
    00092
    00093   for(int i=0; i<buffersize; i++){
    00094     sensorbuffer[i]      = (sensor*) malloc(sizeof(sensor) * this->rsensornumber);
    00095     for(int k=0; k < this->rsensornumber; k++){
    00096       sensorbuffer[i][k]=0;
    00097     }
    00098   }
    00099   blindMotors       = (motor*) malloc(sizeof(motor) * (blindMotorNumber+1));
    00100   for(unsigned int k=0; k < blindMotorNumber; k++){
    00101     blindMotors[k]=0;
    00102   }
    00103
    00104   id                = (sensor*) malloc(sizeof(sensor) * this->rsensornumber);
    00105   first             = (sensor*) malloc(sizeof(sensor) * this->rsensornumber);
    00106   second            = (sensor*) malloc(sizeof(sensor) * this->rsensornumber);
    00107
    00108   if(!noiseGenerator) return false;
    00109   noiseGenerator->init(this->rsensornumber);
    00110   return true;
    00111 }
    00112
    00113 /// Realizes a wiring from robot sensors to controller sensors.
    00114 //   @param rsensors pointer to array of sensorvalues from robot
    00115 //   @param rsensornumber number of sensors from robot
    00116 //   @param csensors pointer to array of sensorvalues for controller (includes derivatives if specified)
    00117 //   @param csensornumber number of sensors to controller
    00118 //   @param noise size of the noise added to the sensors
    00119 bool DerivativeWiring::wireSensors(const sensor* rsensors, int rsensornumber,
    00120                                    sensor* csensors, int csensornumber,
    00121                                    double noise){
    00122   if(rsensornumber != this->rsensornumber || this->csensornumber != csensornumber){
    00123     fprintf(stderr, "%s:%i: Wrong sensornumbers! Robot: Expected: %i, Got: %i Controller: Expected: %i, Got: %i \n",
    00124             __FILE__, __LINE__,
    00125             this->rsensornumber, rsensornumber,
    00126             this->csensornumber, csensornumber);
    00127     return false;
    00128   }
    00129   int index = (time) % buffersize;
    00130   int lastIndex = (time-1) % buffersize;
    00131
    00132   int blocksize = conf.useId + conf.useFirstD + conf.useSecondD;
    00133   if(conf.useId) { // normal sensors values
    00134     memcpy(id, rsensors, sizeof(sensor) * this->rsensornumber);
    00135     noiseGenerator->add(id, -noise, noise);
    00136     for(int i=0; i < this->rsensornumber; i++ ){
    00137       csensors[i*blocksize] = id[i];
    00138     }
    00139   }
    00140
    00141   if(conf.useFirstD || conf.useSecondD){ // calc smoothed sensor values
    00142     for(int i=0; i < this->rsensornumber; i++ ){
    00143       sensorbuffer[index][i] = (1-conf.eps)*sensorbuffer[lastIndex][i] + conf.eps*rsensors[i];
    00144     }
    00145   }
    00146
    00147   if(conf.useFirstD) { // first derivative
    00148     calcFirstDerivative();
    00149     int offset = conf.useId;
    00150     for(int i=0; i < this->rsensornumber; i++ ){
    00151       csensors[i*blocksize+offset] = first[i];
    00152     }
    00153   }
    00154   if(conf.useSecondD) { // second derivative
    00155     calcSecondDerivative();
    00156     int offset = conf.useId + conf.useFirstD;
    00157     for(int i=0; i < this->rsensornumber; i++ ){
    00158       csensors[i*blocksize+offset] = second[i];
    00159     }
    00160     // test  ( if angle near bounce point than set derivative to 0;
    00161     // for(int i=0; i<this->rsensornumber; i++){
    00162     //       if(fabs(*(csensors+i)) > 0.8)
    00163     //  *(csensors+offset+i) = 0;
    00164     //     }
    00165   }
    00166
    00167   if(conf.blindMotorSets > 0) { // shortcircuit of blind motors
    00168     memcpy(csensors+blocksize*this->rsensornumber, blindMotors,
    00169            sizeof(sensor) * blindMotorNumber);
    00170   }
    00171
    00172   time++;
    00173   return true;
    00174 }
    00175
    00176
    00177 /// Realizes wiring from controller motor outputs to robot motors.
    00178 //   @param rmotors pointer to array of motorvalues for robot
    00179 //   @param rmotornumber number of robot motors
    00180 //   @param cmotors pointer to array of motorvalues from controller
    00181 //   @param cmotornumber number of motorvalues from controller
    00182 bool DerivativeWiring::wireMotors(motor* rmotors, int rmotornumber,
    00183                                   const motor* cmotors, int cmotornumber){
    00184
    00185   if (this->cmotornumber!=cmotornumber && rmotornumber <= cmotornumber)
    00186     return false;
    00187   else{
    00188     memcpy(rmotors, cmotors, sizeof(motor)*rmotornumber);
    00189     if(blindMotorNumber!=0){
    00190       memcpy(blindMotors, cmotors + rmotornumber, sizeof(motor)*blindMotorNumber);
    00191     }
    00192     return true;
    00193   }
    00194 }
    00195
    00196 /// f'(x) = (f(x+1) - f(x-1)) / 2
    00197 //  since we do not have f(x+1) we go one timestep in the past
    00198 void DerivativeWiring::calcFirstDerivative(){
    00199   sensor* t   = sensorbuffer[time%buffersize];
    00200   sensor* tmdelay = sensorbuffer[(time-delay)%buffersize];
    00201   for(int i=0; i < rsensornumber; i++){
    00202     first[i] = conf.derivativeScale*(t[i] - tmdelay[i]);
    00203   }
    00204 }
    00205
    00206 /// f'(x) = f(x) - 2f(x-1) + f(x-2)
    00207 void DerivativeWiring::calcSecondDerivative(){
    00208   sensor* t   = sensorbuffer[time%buffersize];
    00209   sensor* tmdelay = sensorbuffer[(time-delay)%buffersize];
    00210   sensor* tm2delay = sensorbuffer[(time-2*delay)%buffersize];
    00211   for(int i=0; i < rsensornumber; i++){
    00212     second[i] = (t[i] - 2*tmdelay[i] + tm2delay[i])*conf.derivativeScale*conf.derivativeScale;
    00213   }
    00214 }
    00215