sim.c

遗传算法和神经网络结合

  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){
    for(int i=0; i < paramNrOfRobots; i++){ 
      
      sprintf(tmpStr,"%s/%s%d-%d.wts",logPath,weightsFile,startGeneration-1,i);
      if ((fp=fopen(tmpStr, "r")) == NULL){ 
	printf("I can't open generation file %s\n\n",tmpStr); 
	exit(1); 
      } 
      printf("Loading weights from file %s\n",tmpStr);
      nN[i]->loadAllIndividsWeights(fp);
      
      fclose(fp);
    }
  }
  else{
    startGeneration=0;
  }
  


/********************************************************************
 *
 *        The main GA loop for startgeneration -> generations
 *
 *
 */

#ifdef GUI
  printf("Waiting for user to press run\n");
  sem_wait(&gui_run);
  /* Test of monitor */
  monNet = new Monitor(300,600);
  for( ii=0; ii < nN[0]->individs[0]->getNrOfNodes(); ii++){
    sprintf(tmpStr,"node %d",ii);
    monNet->addWatch(tmpStr,nN[0]->individs[0]->getActivationPointer(ii));
  }
  monNet->toggleRulers();
  //  monNet->createMonitorWindow();

#endif /* GUI */     

  /*############################################################REAL###############*/
  if(useSerialLine){ /* Real environment */
    real = new Real();
    if(!real->init()){
      fprintf(stderr,"main() init of serial line failed\n");
      exit(-1);
    }
    for(gen=startGeneration;gen<nrOfGenerations;gen++){
      
      if (verboseLevel>0) printf("\nGeneration : %d\n",gen);
      if (verboseLevel>0) printf(" Evaluating primordial soup \n");
      /* Loop trough all individs */
      for(ind=0; ind < nrOfIndivids;ind++){
	for(int i=0; i < paramNrOfRobots; i++){
	  printf(" i%d bot%d\n",ind,i);
	  /* Connect ANN output to robot */
	  annPtr[i] = nN[i]->individs[ind];
	}
#ifdef GUI
	for(ii=0; ii < annPtr[0]->getNrOfNodes(); ii++){
	  monNet->redefineWatch(ii,annPtr[0]->getActivationPointer(ii));
	}
	monNet->toggleColor();
	
#endif
	
	/* The main individ loop for epoch=0 -> amoebaepochs */
	for (epo=0;epo<amoebaEpochs;epo++){
	  
	  /**************************************************/
	  /*   we reset activation values to 0 in the ANN   */
	  for(int i=0; i < paramNrOfRobots; i++)
	    annPtr[i]->resetNet();
	  
	  /**************************************************/
	  /*   Let the robot interact with the environment  */
	  /*   for nrOfSteps time steps                     */
	  for(run = 0; run < nrOfSteps; run++){
	    
	    /**************************************************/
	    /* Let the world generate sensor values for       */
	    /* robot with index 0 -> create inputs values to  */
	    /* ANN   */
	    for(int i=0; i < paramNrOfRobots; i++){
	      real->setInput(i); 	 
	      usleep(5000);	    
	      for(int s=0; s < nrOfSensors; s++){
		annPtr[i]->setInput(s,input[i][mapping[i][s]]);
	      }
	      /**************************************************/
	      /* Activate the ANN for individ n                 */
	      /* WARNING the input array for the active robot   */
	      /* will be undefined after this call.             */
	      /* HOWEVER the simoutput array will be stable     */
	      annPtr[i]->activateNet(); 
	    
	      /**************************************************/
	      /* Run the robot with the output from the ANN     */
	      /*                                                */
	      /* If you are going to use a localistic encoding  */
	      /* of the motor actions this is the place to      */
	      /* modify the code.                               */
	      /*                                                */
	      /* runStep expects two values between 0.0 and 1.0 */
	      /* the first for the left motor and the second for*/
	      /* the right motor                                */
	      i1[i]=annPtr[i]->getOutput(0);
	      i2[i]=annPtr[i]->getOutput(1);
	    
	      real->runStep(i1[i],i2[i],i);  // robot 0
	    }
	    usleep(5000);
	    
#ifdef GUI
	    if(gfxUpdate)
	      monNet->update();
#endif /* GUI */
	    /**************************************************/
	    /* If you want to insert other things that the net*/
	    /* should modify (send it output values to) this  */
	    /* would be a great place do so                   */
	    
	  } /* End of time loop */
	} /* End of epoch loop */
	real->nextIndivid();
      } /* End of individ loop */
    } /* End of generation loop */
  }
  /*############################################################SIMULATED##########*/
  else{/* Simulated environment*/
    for(gen=startGeneration;gen<nrOfGenerations;gen++){
      
      if (verboseLevel>0) printf("\nGeneration : %d\n",gen);
      if (verboseLevel>0) printf(" Running amoeba lives\n");

      /* Loop trough all individs */
      for(ind=0; ind < nrOfIndivids;ind++){
	/*
	 *   Connect ANN output to robot
	 *
	 */
	for(int i=0; i < paramNrOfRobots; i++)
	  annPtr[i] = nN[i]->individs[ind];
#ifdef GUI
	for(ii=0; ii < annPtr[0]->getNrOfNodes(); ii++){
	  monNet->redefineWatch(ii,annPtr[0]->getActivationPointer(ii));
	}
	monNet->toggleColor();
	
#endif
	/* The main individ loop for epoch=0 -> amoebaepochs */
	for (epo=0;epo<amoebaEpochs;epo++){
	  
	  /* Reset robots, new startpos and such thingis */
	  /* make sure  no objects are taken anymore  */
	  /* if GUI redraw world */
	  world->resetEnvironment();
	  
	  for(int i=0; i < paramNrOfRobots; i++){
	  /**************************************************/
	  /*   we reset activation values to 0 in the ANN   */
	    annPtr[i]->resetNet();
	    fitness[i] = 0;	  
	  
	  /**************************************************/
	  /*   We want to be able to see where the robot was*/
	  /*   in the beginning. So save current status   */
	    world->robot[i]->saveState();
	    startPx[i]=world->robot[i]->getX();
	    startPy[i]=world->robot[i]->getY();
	  }
	  
	  /**************************************************/
	  /* Let the world generate sensor values for the   */
	  /* robots                                         */
	  for(int i=0; i < paramNrOfRobots; i++)
	    world->setInput(i); 	 

	  /**************************************************/
	  /*   Let the robot interact with the environment  */
	  /*   for nrOfSteps time steps                     */

	  for(run = 0; run < nrOfSteps; run++){

	    /*********************************************/
	    /* Copy robot activation to networks          */
	    for(int i=0; i < paramNrOfRobots; i++){
	      for(int s=0; s < nrOfSensors; s++){
		annPtr[i]->setInput(s,input[i][mapping[i][s]]);
	      }

	      /**************************************************/
	      /* Activate the ANN for individ n                 */
	      annPtr[i]->activateNet(); 
	    
	      /**************************************************/
	      /* Run the robot with the output from the ANN     */
	      /*                                                */
	      /* If you are going to use a localistic encoding  */
	      /* of the motor actions this is the place to      */
	      /* modify the code.                               */
	      /*                                                */
	      /* runStep expects two values between 0.0 and 1.0 */
	      /* the first for the left motor and the second for*/
	      /* the right motor                                */
	      i1[i]=(float)annPtr[i]->getOutput(0);
	      i2[i]=(float)annPtr[i]->getOutput(1);
	      i3[i]=(float)annPtr[i]->getOutput(2);
	      i4[i]=(float)annPtr[i]->getOutput(3);
	      world->runStep(i1[i],i2[i],i);
	      world->runGripper(i3[i],i4[i],i);

	      /**************************************************/
	      /* If you want to insert other things that the net*/
	      /* should modify (send it output values to) this  */
	      /* would be a great place to do so                */
	      // do_other_nice_thingy(annPtr[i]->getOutput(3));
	    }
	    
#ifdef GUI
	    monNet->update();
	    if(gui_stepping)
	      sem_wait(&gui_step);
#endif /* GUI */

	    /**************************************************/
	    /* Insert your fitness function here              */
	    /**************************************************/
	    
	    for(int i=0; i < paramNrOfRobots; i++){
	      if(world->checkCrash(i)){ /* He messed up we dont want him... */
		if(paramNrOfRobots<2)
		  run=nrOfSteps;          /* so we "end" his life */
		else{
		  world->robot[i]->recallState();
		  fitness[i] -= 1000;
		}
	      }else{
		if(ffitness==1){
		  fitness[i] += world->robot[i]->mot1 + world->robot[i]->mot2;
		}
		if(ffitness==2){
		  // world->inZone(0);  /* this occures in setInput when we have a ground sensor*/
		  fitness[i] += world->robot[i]->mot1 + world->robot[i]->mot2;
		}
	      }

	      world->setInput(i); 	 	      
	    }
	    

#ifdef GUI	     
	    world->drawBots();
#endif /* GUI */
	    /**************************************************/
	    /* We want to be able to see where the robot was  */
	    /* the last time step. So save current status     */
	    
	    for(int i=0; i < paramNrOfRobots; i++){
	      world->robot[i]->saveState();
	    }
	  } /* End of time loop */
	  for(int i=0; i < paramNrOfRobots; i++){
	    /**************************************************/
	    /* Set/(add) fitness for individ(n) and  epoch(e) */
	    /* with new fitness(fitness)                      */
	    if(ffitness==1){ /* euclidian distance between start and stop*/
	      fitness[i] += g_distPoint(startPx[i],startPy[i],
				  world->robot[i]->getX(),world->robot[i]->getY())*10000;
	      fitness[i] = fitness[i] / 100000;
	    }
	    else if(ffitness==2){
	      fitness[i] += 1000 * world->zonesVisited(i);
	      
	    }
	    nN[i]->setFitness(ind,epo,fitness[i]);
	  }
	  world->resetZones();
	  
	} /* End of epoch loop */
      } /* End of individ loop */
      
      if(verboseLevel>0) printf(" Making mutations\n");
      if(verboseLevel>0) printf(" Best results : \n");
      for(int i=0; i < paramNrOfRobots; i++){     
	/* If last, first or saveGeneration then save weights */
	if (gen%saveGeneration == 0 || gen+1 == nrOfGenerations){
	  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);
	}
      
      /* Save fitness results and close file */
 	if (gen <nrOfGenerations){
	  sprintf(tmpStr,"%s/%s%d",logPath,fitnessFile,i);
	  if ((fp=fopen(tmpStr, "a")) == NULL){ 
	    printf("I can't open fitness log file %s for appending",tmpStr); 
	    exit(1); 
	  } 
	  nN[i]->saveBestIndividsFitness(nrOfIndividsToLog,fp,verboseLevel);
	  fclose(fp);
	}
      }
      
      /*********************************************************/
      /* We create the next generation (live long and prosper) */
      /* and reset all old fitness.                            */
      /* If you want to use a new selection method change      */ 
      /* add a case with your selection method (document it in */ 
      /* test.opt)                                             */
      for(int i=0; i < paramNrOfRobots; i++){

	switch(selectionMethod){
	case 0:
	  nN[i]->createNewGenerationWithWeightMutation(nrOfFathers,nrOfKids,bitMutateProbability);
	  break;
	case 1:
	  nN[i]->tournamentSelectionWithMutation(bitMutateProbability);
	  break;
	default:
	  nN[i]->createNewGenerationWithWeightMutation(nrOfFathers,nrOfKids,bitMutateProbability);
	  break;
	}
	
	nN[i]->resetFitness();
      }
    }
  }
#ifdef GUI
  monNet->deleteMonitorWindow();
  delete(monNet);
#endif
}