invertmotorspace.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   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.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 *                                                                         *
 *   $Log: invertmotorspace.cpp,v $
 *   Revision 1.24.6.3  2006/03/29 15:12:09  martius
 *   damping for Model
 *
 *   Revision 1.24.6.2  2006/01/18 16:48:35  martius
 *   stored and restore
 *
 *   Revision 1.24.6.1  2005/11/14 14:55:01  martius
 *   *** empty log message ***
 *
 *   Revision 1.24  2005/10/27 15:46:38  martius
 *   inspectable interface is expanded to structural information for network visualiser
 *
 *   Revision 1.23  2005/10/21 11:51:17  martius
 *   id is now toId
 *
 *   Revision 1.22  2005/10/06 17:08:40  martius
 *   switched to stl lists
 *   Bias noise
 *   correct error_factor for controller learning
 *
 *   Revision 1.21  2005/09/27 10:51:39  martius
 *   scaled error_factor
 *
 *   Revision 1.20  2005/08/22 20:32:09  martius
 *   C- initialisation variable
 *
 *   Revision 1.19  2005/08/03 20:31:14  martius
 *   destructor added
 *
 *   Revision 1.18  2005/07/25 12:15:39  fhesse
 *   now C's and R's are available as internal parameters
 *
 *   Revision 1.17  2005/07/21 11:33:01  fhesse
 *   R as internal parameter instead of C
 *
 *   Revision 1.16  2005/07/21 09:41:02  martius
 *   *** empty log message ***
 *
 *   Revision 1.15  2005/07/12 15:05:54  fhesse
 *   model learning decoupled from conroller learning therefore factorA changed to epsA and eps changed to epsC;
 *   squared and logarithmic error functions (in model and in controller learning) and their switches (params rootE and logaE) added
 *
 *   Revision 1.14  2005/07/07 10:25:34  martius
 *   zeta update disabled
 *
 *   Revision 1.13  2005/07/05 14:47:39  martius
 *   desens deleted.
 *   zetaupdate is continuous
 *
 *   Revision 1.12  2005/07/04 15:22:12  martius
 *   inserted desens (which is not really great!)
 *   used x_smooth also for controller learning
 *
 *   Revision 1.11  2005/07/04 08:31:56  martius
 *   delay is used now
 *
 *   Revision 1.10  2005/07/01 08:54:52  martius
 *   use sensor-compliant (zeta-update) rule
 *   fixed bug in bias update rule
 *
 *   Revision 1.9  2005/06/29 08:31:05  martius
 *   Model BIAS
 *
 *   Revision 1.8  2005/06/23 09:47:52  martius
 *   set more reasonable default values for eps and factor_a
 *
 *   Revision 1.7  2005/06/21 15:31:11  martius
 *   getSensorNumber and getMotorMumber added controller interface
 *
 *   Revision 1.6  2005/06/20 09:04:16  martius
 *   init function added to controller-interface
 *
 *   Revision 1.5  2005/06/16 14:23:42  martius
 *   GPL and name creation from CVS added
 *
 ***************************************************************************/

#include "invertmotorspace.h"

InvertMotorSpace::InvertMotorSpace( int buffersize, double cInit /* = 0.1 */, bool someInternalParams /* = true*/)
  : InvertMotorController(buffersize) {
  
  BNoiseGen = 0;

  this->cInit=cInit;
  this->someInternalParams=someInternalParams;
  
  // prepare name;
  Configurable::insertCVSInfo(name, "$RCSfile: invertmotorspace.cpp,v $", "$Revision: 1.24.6.3 $");
};

InvertMotorSpace::~InvertMotorSpace() {   
  if(BNoiseGen) free(BNoiseGen);
}

//double somevalue(double){ return 0.3;}

void InvertMotorSpace::init(int sensornumber, int motornumber){
  
  number_motors  = motornumber;
  number_sensors = sensornumber;  
  A.set(number_sensors, number_motors);
  C.set(number_motors,  number_sensors);
  R.set(number_motors, number_motors);
  H.set(number_motors,  1);
  B.set(number_sensors, 1);

  BNoiseGen = new WhiteUniformNoise();
  BNoiseGen->init(number_sensors);

  A.toId(); // set a to identity matrix;
  // Matrix Init(number_sensors, number_motors);
  // Init.map(somevalue); // set to constant 0.3;
  A = A;//*0.5; // + Init; //

  C.toId(); // set a to identity matrix;
  C*= cInit;
  x_buffer = new Matrix[buffersize];
  y_buffer = new Matrix[buffersize];
  for (unsigned int k = 0; k < buffersize; k++) {
    x_buffer[k].set(number_sensors,1);
    y_buffer[k].set(number_motors,1);
  }

  initialised = true;
}


/// performs one step (includes learning). Calulates motor commands from sensor inputs.
void InvertMotorSpace::step(const sensor* x_, int number_sensors, 
                            motor* y_, int number_motors){
  fillBuffersAndControl(x_, number_sensors, y_, number_motors);
  if(t>buffersize){
    // get effective motor values which caused present sensor values
    const Matrix& y_effective = y_buffer[(t-int(s4delay))%buffersize];
    const Matrix& x = x_buffer[t%buffersize];
    
    // learn controller with effective input/output
    learnController(x, x_smooth, max(int(s4delay)-1,0));
    
    // learn Model with actual sensors and with effective motors;
    learnModel(x, y_effective);
  }
  // update step counter
  t++;
};

/// performs one step without learning. Calulates motor commands from sensor inputs.
void InvertMotorSpace::stepNoLearning(const sensor* x, int number_sensors, 
                                            motor*  y, int number_motors ){
  fillBuffersAndControl(x, number_sensors, y, number_motors);
  // update step counter
  t++;
};

void InvertMotorSpace::fillBuffersAndControl(const sensor* x_, int number_sensors, 
                                              motor* y_, int number_motors){
  assert((unsigned)number_sensors == this->number_sensors 
         && (unsigned)number_motors == this->number_motors);
  
  Matrix x(number_sensors,1,x_);
  
  // put new input vector in ring buffer x_buffer
  putInBuffer(x_buffer, x);
  
  // averaging over the last s4avg values of x_buffer
  x_smooth = calculateSmoothValues(x_buffer, t < s4avg ? 1 : int(s4avg));
  
  // calculate controller values based on smoothed input values
  Matrix y = calculateControllerValues(x_smooth);

  // put new output vector in ring buffer y_buffer
  putInBuffer(y_buffer, y);

  // convert y to motor* 
  y.convertToBuffer(y_, number_motors);
}

/// learn values H,C
// @param delay 0 for no delay and n>0 for n timesteps delay in the time loop 
void InvertMotorSpace::learnController(const Matrix& x, const Matrix& x_smooth, int delay){ 
  const Matrix& y = y_buffer[t%buffersize];     // current motors
  const Matrix& y_tm1 = y_buffer[(t-1-delay)%buffersize];     // motors, which correspond to x  
  //  const Matrix& x_tm1 = x_buffer[(t-1)%buffersize]; 
  const Matrix xsi    = x -  (A * y_tm1 + B); // Modelling error in sensor-space
  // eta = A^-1 xsi (first shift in motor-space at current time)
  //  we use pseudoinverse U=A^T A -> eta = U^-1 A^T xsi
  const Matrix eta = (A.multTM()^-1) * ( (A^T) * xsi );
  
  // zeta = G' omega
  // RR^T zeta = mu = G'^{-1} eta

  const Matrix z = C * x_smooth + H;  // Todo: check whether x_smooth should be x
  const Matrix g_prime_inv = z.map(g_s_inv);
  R = C * A;
  const Matrix mu = eta.multrowwise(g_prime_inv); // G'^{-1} eta
  const Matrix zeta = (R.multMT()^-1) * mu; // (RR^T)^{-1} * mu  
  
  const Matrix v = (R^T) * zeta;
  const Matrix omega = zeta.multrowwise(g_prime_inv);
  // correction of negled terms (LL^T)^-1 from learning rule, which drive the system out of saturation.
  // double f=1;
  //   for(int i=0; i< g_prime_inv.getM(); i++){
  //     f *= g_prime_inv.val(i,0);
  //   }
  //   const Matrix omega = eta * f; 
  const Matrix alpha = A * v;
  const Matrix beta = omega.multrowwise(eta).multrowwise(y) * -2;

  const Matrix zmodel = R * y + H;
  Matrix C_update; 
  Matrix H_update;

  // apply updates to H,C
  if(zetaupdate==0){
    // delta C = eps * (zeta * alpha^T + beta x^T)    
    C_update = (zeta * (alpha^T) + beta * (x_smooth^T)) * epsC;
    H_update = (beta * epsC); // delta H = beta
  }else{
    // delta C = eps * (zeta * alpha^T + beta x^T + zeta x^T)    
    C_update = (zeta * (alpha^T) + (beta + zeta*zetaupdate) * (x_smooth^T)) * epsC;
    H_update = (beta + zeta*zetaupdate) * epsC;     // delta H = beta + zeta  
  }  

  if (!logaE==0){   // using logarithmic error E=ln(v^T*v)
    double temp= 1/(v.multTM().val(0,0)+0.0000001)*0.01;
    C_update *= temp;
    H_update *= temp;
  }
  if (!rootE==0){  // using root error E=(v^T*v)^(1/2)
    double temp= 1/sqrt(v.multTM().val(0,0)+0.0000001)*0.1;
    C_update *= temp;
    H_update *= temp;
  }
  
  C += C_update.map(squash);
  H += H_update.map(squash);        
};

// normal delta rule  
void InvertMotorSpace::learnModel(const Matrix& x, const Matrix& y){
  Matrix xsi = x -  (A * y + B);

  Matrix A_update;
  Matrix B_update;
  A_update=(( xsi*(y^T) ) * epsA ); 
  Matrix B_noise  = noiseMatrix(B.getM(),B.getN(), *BNoiseGen, -noiseB, noiseB); // noise for bias
  B_update=(  xsi * epsA + B_noise) * factorB; 

  if (!logaE==0){   // using logarithmic error E=ln(xsi^T*xsi)
    Matrix xsi2 = (xsi^T)*xsi;
    double temp= 1/(xsi2.val(0,0)+0.0000001);
    A_update *= temp;
    B_update *= temp;
  }
  if (!rootE==0){  // using root error E=(xsi^T*xsi)^(1/2)
    const Matrix xsi2=(xsi^T)*xsi;
    double temp= 1/sqrt(xsi2.val(0,0)+0.0000001);
    A_update *= temp;
    B_update *= temp;
  }

  A += A_update.map(squash);
  B += B_update.map(squash);

};

/// calculate controller outputs 
/// @param x_smooth smoothed sensors Matrix(number_channels,1) 
Matrix InvertMotorSpace::calculateControllerValues(const Matrix& x_smooth){
  return (C*x_smooth+H).map(g);  
};

/// calculate delayed values
Matrix InvertMotorSpace::calculateDelayedValues(const Matrix* buffer, 
                                                       int number_steps_of_delay_){
  // number_steps_of_delay must not be smaller than buffersize
  assert ((unsigned)number_steps_of_delay_ < buffersize);  
  return buffer[(t - number_steps_of_delay_) % buffersize];
};

Matrix InvertMotorSpace::calculateSmoothValues(const Matrix* buffer, 
                                               int number_steps_for_averaging_){
  // number_steps_for_averaging_ must not be larger than buffersize
  assert ((int)number_steps_for_averaging_ <= buffersize);

  Matrix result(buffer[t % buffersize]);
  for (int k = 1; k < number_steps_for_averaging_; k++) {
    result += buffer[(t - k) % buffersize];
  }
  result *= 1/((double) (number_steps_for_averaging_)); // scalar multiplication
  return result;
};


/** stores the controller values to a given file. */
bool InvertMotorSpace::store(const char* filename){  
  // save matrix values
  FILE* f;
    if(!(f = fopen(filename, "w"))) return false;
  storeMatrix(C,f);
  storeMatrix(H,f);
  storeMatrix(A,f);
  storeMatrix(B,f);
  fclose(f);
  return true;
}

/** loads the controller values from a given file. */
bool InvertMotorSpace::restore(const char* filename){
  // save matrix values
  FILE* f;  
  if(!(f = fopen(filename, "r"))) return false;
  restoreMatrix(C,f);
  restoreMatrix(H,f);
  restoreMatrix(A,f);
  restoreMatrix(B,f);
  fclose(f);
  t=0; // set time to zero to ensure proper filling of buffers
  return true;
}


list<Inspectable::iparamkey> InvertMotorSpace::getInternalParamNames() const{
  list<iparamkey> keylist;
  if(someInternalParams){  
    keylist+=store4x4AndDiagonalFieldNames(A,"A");
    keylist+=store4x4AndDiagonalFieldNames(C,"C");
    keylist+=store4x4AndDiagonalFieldNames(R,"R");
  }else{
    keylist+=storeMatrixFieldNames(A,"A");
    keylist+=storeMatrixFieldNames(C,"C");
    keylist+=storeMatrixFieldNames(R,"R");
  }
  keylist+=storeVectorFieldNames(H,"H");
  keylist+=storeVectorFieldNames(B,"B");
  return keylist;
}

list<Inspectable::iparamval> InvertMotorSpace::getInternalParams() const{
  list<iparamval> l;
  if(someInternalParams){  
    l+=store4x4AndDiagonal(A);
    l+=store4x4AndDiagonal(C);
    l+=store4x4AndDiagonal(R);
  }else{
    l+=A.convertToList();
    l+=C.convertToList();
    l+=R.convertToList();
  }
  l+=H.convertToList();
  l+=B.convertToList();
  return l;
}

list<Inspectable::ILayer> InvertMotorSpace::getStructuralLayers() const {
  list<Inspectable::ILayer> l;
  l+=ILayer("x","", number_sensors, 0, "Sensors");
  l+=ILayer("y","H", number_motors, 1,"Motors");
  l+=ILayer("xP","B", number_sensors, 2,"Prediction");
  return l;
}

list<Inspectable::IConnection> InvertMotorSpace::getStructuralConnections() const {
  list<Inspectable::IConnection> l;
  l+=IConnection("C", "x", "y");
  l+=IConnection("A", "y", "xP");
  return l;
}

---