sim.c

遗传算法和神经网络结合

/*
  YAKS, 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"
#include "terminal.h"

/* GUI related includes*/
#ifdef GUI
#include "monitor.h"
#include <pthread.h>
#include <semaphore.h>
#include <support.h>
#include <glib.h>
#include "world.h"

pthread_t tid;

sem_t gui_run;
sem_t gui_step;
sem_t gui_ready;

                 
#endif /* GUI */

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

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

int ind;            /* running Individ    */
int gen;            /* running Generation */
int epo;            /* running Epoch      */
int run;            /* running Step (interaction steps between the robot and the world */


/* double   P90,P180,P360; */
#ifdef GUI
/* Callback functions for GUI */
void sim_redrawWorld(){
  printf("No - that would cause deadlock =) \n");
  //  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_loadNextIndivid(){
}
#endif /* GUI */


void sim_shutdown(){
  exit(0);
}

void sim_nextIndivid(){
  ind++;
  if(ind>nrOfIndivids)
    ind=0;
  for(int i=0; i < paramNrOfRobots; i++){
    /* Connect ANN output to robot */
    annPtr[i] = nN[i]->individs[ind];
  }
}

void sim_prevIndivid(){
  ind--;
  if(ind<0)
    ind=nrOfIndivids-1;
  for(int i=0; i < paramNrOfRobots; i++){
    /* Connect ANN output to robot */
    annPtr[i] = nN[i]->individs[ind];
  }
}

void sim_saveweights(){
  FILE *fp;
  char tmpStr[400];
  for(int i=0; i < paramNrOfRobots; i++){     
    sprintf(tmpStr,"%s/%s%d-%d.wts",logPath,weightsFile,gen,i);
    if ((fp=fopen(tmpStr, "w")) == NULL){ 
      printf("I can't open weight file %s for writing\n",tmpStr); 
      exit(1); 
    } 
    printf("Saving weights to file %s\n",tmpStr);
    nN[i]->saveAllIndividsWeights(fp);
    fflush(fp);
    fclose(fp);
  }
}

void sigCtrlC(int dontCare){
  Terminal *term;
  term = new Terminal();
  term->addItem("q","Simulator shutdown",sim_shutdown);
  term->addItem("n","Next individ",sim_nextIndivid);
  term->addItem("p","Prev individ",sim_prevIndivid);
  term->addItem("s","Save current generation",sim_saveweights);
  if(useSerialLine){
    for(int i=0; i < paramNrOfRobots; i++){
      /* Stop the robots */
      fprintf(stderr,"Telling bot %d to stop\n",i);
      real->runStep(0.5,0.5,i);
    }
  }
  term->runTerminal(); 
  delete(term);
}

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;
  if(useRodSensor)
    input += ROD_RESOLUTION;
  return inputs;
}

int main(int argc,char **argv){

  double *fitness;
  int nrOfSensors,nrOfInputs;
  float *i1,*i2, *i3, *i4;
  float *startPy,*startPx;
  char worldFileStr[200];
  char tmpStr[200];
#ifdef GUI
  Monitor *monNet;
  int ii;
#endif

  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;
  }

  i1 = new float[paramNrOfRobots];
  i2 = new float[paramNrOfRobots];
  i3 = new float[paramNrOfRobots];
  i4 = new float[paramNrOfRobots];

  startPy = new float[paramNrOfRobots];
  startPx = new float[paramNrOfRobots];
  annPtr = (class Ann**) malloc(sizeof(class Ann*)*paramNrOfRobots);
  fitness = (double*) malloc(sizeof(double)*paramNrOfRobots);
  nN = (class Ga **) malloc(sizeof(class Ga*)*paramNrOfRobots);
  for(int i=0; i < paramNrOfRobots; i++){
    orginal = new Ann();
    buildNetwork[i%NETWORKS](orginal);
    /* We create the first generation                  */
    nN[i] = new Ga(nrOfIndivids,amoebaEpochs);
    /* and specifies how the individs ought to look    */
    /* with our precreated neural network              */
    nN[i]->createIndivids(orginal);           
    /* Randomize all weights                           */
    nN[i]->mutateIndivids(50);
    /* Discard out orginal net we dont need it anymore */
    delete(orginal);
  }

  /* 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[0]->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++){
    nrOfSensors = 0;
    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;
    }
    if(useRodSensor>0){
      for(int l=0; l < ROD_RESOLUTION; l++){
	mapping[robotNr][nrOfSensors+l]=RodSensor+l;
      }
      nrOfSensors+=ROD_RESOLUTION;

    }
  }

  /* 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 necessary sensor sample files will be loaded to 
   */
  sprintf(worldFileStr,"%s/%s",worldPath,worldFile);
  world->loadWorld(worldFileStr);

  /* Our ctrl^c handler that starts a terminal */
  signal(SIGINT,sigCtrlC);  
#ifdef GUI

  g_thread_init(NULL);

  gtk_set_locale ();
  gtk_init (&argc, &argv);
  //  gdk_init(&argc, &argv);

  add_pixmap_directory ((const gchar*)"./pixmaps");
  add_pixmap_directory ((const gchar*)"./pixmaps");

  sim_setWorldSize();

  sem_init(&gui_ready,1,0); 
  sem_init(&gui_run,1,0);
  sem_init(&gui_step,1,1);

  pthread_create(&tid, NULL,(void *(*)(void *))gui_init,NULL);

  printf("Wait for gui to launch\n");