simt.c

遗传算法和神经网络结合

/*
  Optann, a Khepera simulator including a separate GA and ANN 
  (Genetic Algoritm, Artificial Neural Net).
  Copyright (C) 2000  Johan Carlsson ([e-mail])
  
  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 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.

 */

/* Standard libraries */
#include <stdio.h>
#include <unistd.h>
#include <values.h>
#include <stdio.h>
#include <fcntl.h>
#include <termio.h>
#include <time.h>

/* Simulator related includes*/ 
#include "ga.h"
#include "ann.h"
#include "buildn.h"
#include "geom.h"
#include "environ.h"
#include "param.h"
#include "real.h"

/* GUI related includes*/
#ifdef GUI

#include "gui.h"
#include "monitor.h"
#include <pthread.h>
#include <semaphore.h>

pthread_t tid;
                 
#endif /* GUI */


/* The world and such alike */
Environ *world;

/*  Network, ga and such  */
Ga *nN;
Ann *orginal;

/* double   P90,P180,P360; */
#ifdef GUI
/* Callback functions for GUI */
void sim_redrawWorld(){
  world->redrawWorld();
}

float sim_getRobotX(){
  return world->robot[0]->getX();
}

float sim_getRobotY(){
  return world->robot[0]->getY();
}

void sim_setRobotX(float x){
  world->robot[0]->setX(x);
}
void sim_setRobotY(float y){
  world->robot[0]->setY(y);
}

void sim_setWorldSize(){
  gui_setWorldSize((int)(world->dx*xScale),
		   (int)(world->dy*xScale));
}

void sim_toggleGFX(){
  if(gfxUpdate){
    gfxUpdate=0;
  }
  else{
    gfxUpdate=1;
    world->redrawWorld();
  }
}

void sim_toggleUSER(){
  if(userUpdate){
    userUpdate=0;
  }
  else{
    userUpdate=1;
  }
}

#endif /* GUI */


int countInputs(){
  int inputs=0;
  if (useEnergy==1)
    inputs++;
  if (gripperSensor==1)
    inputs++;
  if (useCompass==1)
    inputs++;
  if (useGripper==1)
    inputs++;
  if (useGround == 1)
    inputs++;
  if(useLightbulbSensors>0)
    inputs+=8;
  inputs += nrOfFrontInfrared;
  inputs += nrOfBackInfrared;
  return inputs;
}

int main(int argc,char **argv){
  double fitness;
  int ii;
  int n;              /* running Individ     */
  int g;              /* running Generation  */
  int e;              /* running Epoch       */
  int run;            /* running Step (interaction steps between the robot and the world */
  float i1,i2;
  float startPy,startPx;
  int nrOfSensors,nrOfInputs;
  char worldFileStr[200];
  char tmpStr[200];
#ifdef GUI
  Monitor *monNet,*monLinks;
#endif

  class Ann *annPtr;
  class Real *real;
  FILE *fp;
  seed=time(NULL);
  srand(seed);
  setlinebuf(stdout);
  setlinebuf(stderr);
  if(argc > 1)
    loadParams(argv[1]);
  else{
    fprintf(stderr,"No option file specified\n");
    fprintf(stderr,"use: sim optionFile.opt \n\n");
    return -1;
  }
  orginal = new Ann();
  //  buildNetwork(orginal);
  /* Build a network suitable for Tom's architecture */

  // First we build the first network
  // input layer
#define IA 8   // nodes in A input layer
#define OA1 2  // nodes in A output to C and B
#define OA2 2  // nodes in A output to Robot
#define OB1 (IA*OA1+IA*OA2) // nodes in B output to A IA->OA1 weights
#define OC1 OB1*OA1  // nodes in C output to B

  for(int i=0; i < IA; i++)
    orginal->addNeuron(10,INPUT,SIGMOID);
  for(int i=0; i < OA2; i++)
    orginal->addNeuron(40,OUTPUT,SIGMOID);

  orginal->connectLayer(10,40);
  /*  for(int i=0; i < OA1; i++)
    orginal->addNeuron(41,INTERN,SIGMOID);
  
  for(int i=0; i < OB1; i++)
    orginal->addNeuron(50,SCN,LINEAR);

  orginal->connectLayer(41,50);
  orginal->connectLayerWithSCN(10,40,50);
  orginal->connectLayerWithSCN(10,41,50);*/

  /*
  for(int i=0; i < IA; i++)
    orginal->addNeuron(10,INPUT,SIGMOID);
  for(int i=0; i < OA1; i++)
    orginal->addNeuron(50,INTERN,SIGMOID);
  for(int i=0; i < OA2; i++)
    orginal->addNeuron(51,OUTPUT,SIGMOID);
  for(int i=0; i < OB1; i++)
    orginal->addNeuron(70,SCN,LINEAR);
  for(int i=0; i < OC1; i++)
     orginal->addNeuron(80,SCN,LINEAR);
   
  orginal->connectLayer(50,80);
  orginal->connectLayerWithSCN(50,70,80);
  orginal->connectLayerWithSCN(10,50,70);
  orginal->connectLayerWithSCN(10,51,70);*/


  nN = new Ga(nrOfIndivids,amoebaEpochs);/* We create the first generation                  */
  nN->createIndivids(orginal);           /* and specifies how the individs ought to look    */
                                         /* with our precreated neural network              */
  nN->mutateIndivids(50);                /* Randomize all weights                           */
  delete(orginal);                       /* Discard out orginal net we dont need it anymore */

  /* we are ready to rock'n'roll, all */
  /* individs have fitness of zero    */
#ifdef DEBUG_INFO
  printf("Init ANN ready \n");
#endif /* DEBUG_INFO */

  /* Now some ANN sorting out                                
   * -------------------------------                          
   * We need to know how many inputs there's for each ANN,   
   * we find this out by checking a individ.              
   *
   * What sensor are we going to use, sum them up and compare
   * the number of inputs with the number of sensors  
   */
  nrOfInputs =  nN->individs[0]->getNrOfInputNodes();
  nrOfSensors = countInputs();

  if(nrOfSensors>nrOfInputs){
    fprintf(stderr,"Number of ANN inputs doesn't match number of sensors to use on the robot\n");
    fprintf(stderr,"Net inputs %d < %d simulator sensors used\n",nrOfInputs,nrOfSensors);
    exit(-1);
  }
  
  /* Create a input mapping ANN <-> simulator
   *
   * Starting with the six front infrared sensors 
   */
  for(int robotNr=0; robotNr < paramNrOfRobots; robotNr++){
    switch(nrOfFrontInfrared){
    case 6:
      mapping[robotNr][0]=0;
      mapping[robotNr][1]=1;
      mapping[robotNr][2]=2;
      mapping[robotNr][3]=3;
      mapping[robotNr][4]=4;
      mapping[robotNr][5]=5;
      nrOfSensors = 6;
      break;
    case 4:
      mapping[robotNr][0]=0;
      mapping[robotNr][1]=1;
      mapping[robotNr][2]=2;
      mapping[robotNr][3]=3;
      nrOfSensors = 4;
      break;
    case 2:
      mapping[robotNr][0]=0;
      mapping[robotNr][1]=1;
      nrOfSensors = 2;
      break;
    default:
      printf("Incorrect number of front infrared sensors specified - %d choosed\n",nrOfFrontInfrared);
      printf("Possible choices are 2, 4 or 6\n");
      exit(-1);
      break;
    }
    
    /*
     * Next is the back infrared sensor
     */
    if(nrOfBackInfrared>0){
      mapping[robotNr][nrOfSensors] = 6;
      mapping[robotNr][nrOfSensors+1] = 7;
      nrOfSensors += 2;
    }
    /*
     *   And then we have all nifty robot sensors
     *
     */
    if(gripperSensor>0){
      mapping[robotNr][nrOfSensors++] = GripperSensor;
    }
    if(useCompass>0){
      mapping[robotNr][nrOfSensors++] = CompassSensor;
    }
    if(useGround>0){
      mapping[robotNr][nrOfSensors++] = GroundSensor;
    }
    if(useEnergy>0){
      mapping[robotNr][nrOfSensors++] = EnergySensor;
    }
    if(useLightbulbSensors>0){
      for(int l=0; l < 8; l++){
	mapping[robotNr][nrOfSensors+l]=LightBulbSensor+l;
      }
      nrOfSensors+=8;
    }
  }
  /* Prepare the world */
  /* First we create the world */
  world = new Environ();
  /* We tell the world to load the world specified in the option file 
   * and all nescary sensor sample files will be loaded to 
   */
  sprintf(worldFileStr,"%s/%s",worldPath,worldFile);
  world->loadWorld(worldFileStr);
  
#ifdef GUI
  
  pthread_create(&tid, NULL,(void *(*)(void *))guiStartUp, argv);
  sim_setWorldSize();
  printf("Wait for gui to launch\n");
  sem_init(&gui_ready,0,0); 
  sem_wait(&gui_ready);
  printf("GUI ready -> GO !!! \n");
#endif /* GUI */

  /* Some sane tests */
  if(startGeneration > nrOfGenerations){
    fprintf(stderr,"\n\n********\nstartGeneration > nrOfGenerations \n");
    fprintf(stderr,"give me a break man ! I cant go back in time...\n********\n");
    exit(-1);
  }


  if (startGeneration >= 0){
    sprintf(tmpStr,"%s/%s%d.wts",logPath,weightsFile,startGeneration-1);
    if ((fp=fopen(tmpStr, "r")) == NULL){ 
      printf("I can't open generation file %s\n\n",tmpStr); 
      exit(1); 
    } 
    nN->loadAllIndividsWeights(fp);