dinvert3channelcontroller.hpp

Go to the documentation of this file.

/* 
 *   LICENSE:                                                              *
 *   This work is licensed under the Creative Commons                      *
 *   Attribution-NonCommercial-ShareAlike 2.5 License. To view a copy of   *
 *   this license, visit http://creativecommons.org/licenses/by-nc-sa/2.5/ *
 *   or send a letter to Creative Commons, 543 Howard Street, 5th Floor,   *
 *   San Francisco, California, 94105, USA.                                *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.                  *
*/

double** allocateMatrix(int m, int n){
  double** mat=(double**)malloc(m*sizeof(double*));
  for(int i=0; i < m; i++){
    mat[i]=(double*)malloc(n*sizeof(double));
  }
  return mat;
}

void freeMatrix(double** mat, int m){
  for(int i=0; i < m; i++){
    free(mat[i]);
  }
  free(mat);
}


public: 

/// return the name of the object (with version number)
virtual constparamkey getName() const {  
  return name; 
}


virtual void init(int sensornumber, int motornumber){
  
}


DInvert3ChannelController(int numberchannels, int buffersize){
  
  eps=0.7;
  rho=0.0; 
  s4delay=1;
  s4avg=1;
  delta=0.01;
  factor_a=1.0;
  t=0;
  m=0.0;
    
  NUMBER_CHANNELS=numberchannels;
  BUFFER_SIZE=buffersize;
  // allocate field;
  A = allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
  C = allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
  h = (double*)malloc(NUMBER_CHANNELS*sizeof(double));
 
  x_buffer = allocateMatrix(BUFFER_SIZE, NUMBER_CHANNELS);
  y_buffer = allocateMatrix(BUFFER_SIZE, NUMBER_CHANNELS);

  xsi4E = (double*)malloc(NUMBER_CHANNELS*sizeof(double));
  xsi4Model = (double*)malloc(NUMBER_CHANNELS*sizeof(double));

  // internals
  x_smooth = (double*)malloc(NUMBER_CHANNELS*sizeof(double));
  x_effective = (double*)malloc(NUMBER_CHANNELS*sizeof(double));
  y_effective = (double*)malloc(NUMBER_CHANNELS*sizeof(double));
  Q_buf1 = allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
  Q_buf2 = allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
  L = allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
  z = (double*)malloc(NUMBER_CHANNELS*sizeof(double));


  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      h[i] = 0.0;
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          if (i == j)
            {
              A[i][j] = (rand()/10.0)/RAND_MAX;
              C[i][j] = (rand()/10.0)/RAND_MAX;
            }
          else
            {
              A[i][j] = (rand()/10.0)/RAND_MAX;
              C[i][j] = (rand()/10.0)/RAND_MAX;
            }
        }
    }
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      for (int k = 0; k < BUFFER_SIZE; k++)
        {
          x_buffer[k][i] = 0;
          y_buffer[k][i] = 0;   
        }
    }
  // prepare name;
  Configurable::insertCVSInfo(name, "$RCSfile: dinvert3channelcontroller.hpp,v $", 
                                    "$Revision: 1.5 $");
    
  /*    // print initial values
        std::cout<<"Constructor of RobotLearnControl:"<<std::endl;
        std::cout<<"init: epsilon="<<eps<<std::endl;     
        std::cout<<"init: rho="<<rho<<std::endl;         
        std::cout<<"init: steps4delay="<<steps4delay<<std::endl; 
        std::cout<<"init: steps4avg="<<steps4avg<<std::endl;     
        std::cout<<"init: delta="<<delta<<std::endl; 
        std::cout<<"init: m (for calculation of E)="<<m<<std::endl; 
  */    
};

/// performs one step (includes learning). Calulates motor commands from sensor inputs.
virtual void step(const sensor* x_, int sensornumber, motor* y_, int motornumber){
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      x_smooth[i] = 0.0;
      x_effective[i] = 0.0;
      y_effective[i] = 0.0;
    }

  // put new input value in ring buffer x_buffer
  putInBuffer(x_buffer, x_);

  // averaging over the last steps4avg values of x_buffer
  calculateSmoothValues(x_buffer, s4avg, x_smooth);
    
  // calculate controller values based on smoothed input values
  calculateControllerValues(x_smooth, y_);

  // put new output value in ring buffer y_buffer
  putInBuffer(y_buffer, y_);

  // calculate effective input/output, which is (actual-steps4delay) element of buffer
  calculateDelayedValues(x_buffer, s4delay, x_effective);    
  calculateDelayedValues(y_buffer, s4delay, y_effective);
 
  // learn controller with effective input/output
  learn(x_, x_effective, y_effective);
    
  // learn model with actual input and effective output (that produced the actual input)
  learnModel(x_, y_effective);

  // update step counter
  t++;
};


/// performs one step without learning. Calulates motor commands from sensor inputs.
virtual void stepNoLearning(const sensor* x_, int sensornumber, motor* y_, int motornumber){
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      x_smooth[i] = 0.0;
    }


  // put new input value in ring buffer x_buffer
  putInBuffer(x_buffer, x_);

  // averaging over the last steps4avg values of x_buffer
  calculateSmoothValues(x_buffer, s4avg, x_smooth);
    
  // calculate controller values based on smoothed input values
  calculateControllerValues(x_smooth, y_);

  // put new output value in ring buffer y_buffer
  putInBuffer(y_buffer, y_);

  // update step counter
  t++;
};
  
virtual int getInternalParamNames(paramkey*& keylist){
  return 0;
}

virtual int getInternalParams(paramval* vallist, int length){
  int len = 0;
  return len;
}
 
protected:
virtual void inverseMatrix(double** Q, double** Q_1){
  // Berechne Inverse von Q

  // only if NUMBER_CHANNELS<4
  assert(NUMBER_CHANNELS<4);
  if (NUMBER_CHANNELS==2){

    double det = Q[0][0] * Q[1][1] - Q[0][1] * Q[1][0];
    Q_1[0][0] = Q[1][1] / det;
    Q_1[1][1] = Q[0][0] / det;
    Q_1[0][1] = -Q[0][1] / det;
    Q_1[1][0] = -Q[1][0] / det;
      
  }
    
    
  if (NUMBER_CHANNELS==3){  
      
    double  detQ=0  ;
    double** Q_adjoint=Q_buf1;
      
    //calculate the inverse of Q
    Q_adjoint[0][0]=Q[1][1]*Q[2][2]-Q[1][2]*Q[2][1] ;
    Q_adjoint[0][1]=(Q[1][2]*Q[2][0]-Q[1][0]*Q[2][2]) ;
    Q_adjoint[0][2]=Q[1][0]*Q[2][1]-Q[1][1]*Q[2][0] ;
    Q_adjoint[1][0]=(Q[2][1]*Q[0][2]-Q[0][1]*Q[2][2]) ;
    Q_adjoint[1][1]=Q[0][0]*Q[2][2]-Q[0][2]*Q[2][0] ;
    Q_adjoint[1][2]=(Q[0][1]*Q[2][0]-Q[0][0]*Q[2][1]) ;
    Q_adjoint[2][0]=Q[0][1]*Q[1][2]-Q[1][1]*Q[0][2] ;
    Q_adjoint[2][1]=(Q[1][0]*Q[0][2]-Q[0][0]*Q[1][2]) ;
    Q_adjoint[2][2]=Q[0][0]*Q[1][1]-Q[0][1]*Q[1][0] ;
    detQ=Q[0][0]*Q_adjoint[0][0]+Q[0][1]*Q_adjoint[0][1]+Q[0][2]*Q_adjoint[0][2] ;
    for(int i=0; i<NUMBER_CHANNELS; i++){
      for(int j=0; j<NUMBER_CHANNELS; j++) {                                                     
        Q_1[i][j]=(Q_adjoint[j][i])/detQ  ;
      }
    }       
  } 
};


virtual double calculateE(const double *x_, const double *x_delay, const  double *y_delay) {
  double** Q = Q_buf1;
  double** Q_1= Q_buf2;
    

  // Calculate z based on the delayed inputs since the present input x is
  // produced by the outputs tau time steps before
  // which on their hand are y = K(x_D)
  // due to the delay in the feed back loop.

  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      z[i] = h[i];
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          z[i] += C[i][j] * x_delay[j];
        }
    }

  // Berechne Matrix L
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          L[i][j] = 0.0;
          for (int k = 0; k < NUMBER_CHANNELS; k++)
            {
              L[i][j] += A[i][k] * g_s(z[k]) * C[k][j];
            }
        }
    }

  // Berechne Q=LL^T
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          Q[i][j] = 0.0;
          for (int k = 0; k < NUMBER_CHANNELS; k++)
            {
              Q[i][j] += L[i][k] * L[j][k];
            }
          if (i == j)
            Q[i][j] += rho / NUMBER_CHANNELS; // Regularisation
        }
    }

  // Berechne Inverse von Q
  inverseMatrix(Q, Q_1);

  // Berechne xsi
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      xsi4E[i] = x_[i];
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          xsi4E[i] -= A[i][j] * y_delay[j];  // using old y value -> no influence of changes (adding delta) in controller
          // very very strange!!!!
          //xsi4E[i] -= A[i][j] * g(z[j]);     // using recalculating y -> influence of changes (adding delta) in controller
        }
    }
  double E = 0;
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          E += xsi4E[i] * Q_1[i][j] * xsi4E[j];
        }
    }

  double E_s=0;
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          E_s += (A[i][j]*g(z[j]) - x_[i]) * (A[i][j]*g(z[j]) - x_[i]);
        }
    }

  E=(1-m)*E+ m*E_s;
  return (E);

};


virtual void learn(const double *x_, const double *x_delay, const double *y_delay)
{
  //    double A_update[NUMBER_CHANNELS][NUMBER_CHANNELS];
  double** C_update=allocateMatrix(NUMBER_CHANNELS,NUMBER_CHANNELS);
  double* h_update=(double*)malloc(NUMBER_CHANNELS*sizeof(double));

  double E_0 = calculateE(x_,x_delay, y_delay);

  // calculate updates for h,C,A
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      h[i] += delta;
      h_update[i] = -eps * (calculateE(x_,x_delay, y_delay) - E_0) / delta;
      h[i] -= delta;
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          C[i][j] += delta;
          C_update[i][j] = -eps * (calculateE(x_,x_delay, y_delay) - E_0) / delta;
          C[i][j] -= delta;
          //A[i][j] += delta;
          //A_update[i][j] = -eps * a_factor * (calculateE(x_delay, y_delay) - E_0) / delta;
          //A[i][j] -= delta;
        }
    }

  // apply updates to h,C,A
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      h[i] += squash(h_update[i]);
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          C[i][j] += squash(C_update[i][j]);
          //A[i][j] += squash(A_update[i][j]);
        }
    }
  freeMatrix(C_update, NUMBER_CHANNELS);
  free(h_update);
};

  
virtual void learnModel(const double *x_actual, double *y_effective){
  /*    double z[N_output];    
        for(int i=0; i<N_output; i++){
        z[i]=h[i];
        for(int j=0; j<N_input; j++) {
        z[i]+=C[i][j]*x_D[j];
        }
        }
  */
  // Berechne xsi
  for(int i=0; i<NUMBER_CHANNELS; i++){
    xsi4Model[i]=x_actual[i];  
    for(int j=0; j<NUMBER_CHANNELS; j++){
      xsi4Model[i]-= A[i][j]*y_effective[j];
    }    
  }

  for(int i=0; i<NUMBER_CHANNELS; i++){
    for (int j=0; j<NUMBER_CHANNELS; j++){   
      A[i][j]+=squash( (factor_a*eps*0.2) *xsi4Model[i] * y_effective[j]) ;
    }
  }
};

  

  
  
/// calculate delayed values
virtual void calculateDelayedValues(double** source, paramval number_steps_of_delay_, double *target)  
{
  // number_steps_of_delay_ must not be larger than BUFFER_SIZE
  assert ((int)number_steps_of_delay_ < BUFFER_SIZE);

  // get delayed value from ring buffer

  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      target[i] = source[(t - (int)number_steps_of_delay_ + BUFFER_SIZE) % BUFFER_SIZE][i];
    }
};

virtual void calculateSmoothValues(double** source, paramval number_steps_for_averaging_, double *target)
{
  // number_steps_for_averaging_ must not be larger than BUFFER_SIZE
  assert ((int)number_steps_for_averaging_ <= BUFFER_SIZE);

  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      target[i] = 0.0;
      for (int k = 0; k < (int)number_steps_for_averaging_; k++)
        {
          target[i] += source[(t - k + BUFFER_SIZE) % BUFFER_SIZE][i]/ (double) (number_steps_for_averaging_);
        }
    }
};



/// calculate controller ouptus
virtual void calculateControllerValues(double *x_smooth, double *y)  
{
  double z[NUMBER_CHANNELS];

  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      z[i] = h[i];
      for (int j = 0; j < NUMBER_CHANNELS; j++)
        {
          z[i] += C[i][j] * x_smooth[j];
        }
      y[i] = g(z[i]);
    }
};
  
  
// put new value in ring buffer
virtual void putInBuffer(double** buffer, const double *values){  
  for (int i = 0; i < NUMBER_CHANNELS; i++)
    {
      buffer[(t+BUFFER_SIZE)% BUFFER_SIZE][i] = values[i];
    }
};

---