environ.cc

神经网络和遗传算法组合应用

 * Check if a robot has chrashed with something.
 * @param p1 Robot X coordinate.
 * @param p2 Robot Y coordinate.
 * @return 1 if the robot has crashed with somethin otherwise 0.
 * @attention Not gripper safe.
 */
int Environ::checkCrash(float p1, float p2) const{
  int i;
  double  dist;
  double  mindist;
  
  mindist = robot[0]->getRadius();
  for(i=0;i<nrOfWalls;i++){
    dist = wall[i]->distR(p1,p2);
    if (dist < mindist)
      return 1;
  }
  for(i=0;i<nrOfObst;i++){
    dist = g_distPoint(p1,p2,obst[i]->x,obst[i]->y)-obst[0]->r;
    if (dist < mindist)
      return 1;
  }
  for(i=0;i<nrOfSObst;i++){
    if (sobst[i]->taken == 0.0){
      dist = g_distPoint(p1,p2,sobst[i]->x,sobst[i]->y)-sobst[0]->r;
      if (dist < mindist)
	return 1;
    }
  }
  for(i=0; i < nrOfRobots; i++){
    dist = g_distPoint(p1,p2,robot[i]->getX(),robot[i]->getY())-robot[i]->getRadius();
    if (dist < mindist)
      return 1;
  }
  return(0);
}


/**
 * Check if a robot has chrashed with something.
 * @param whichRobot Which robot.
 * @return 1 if the robot has crashed with somethin otherwise 0.
 */
int Environ::checkCrash(int whichRobot) const{
  int i;
  double  dist;
  double  mindist;
  float   p1,p2;
  double direction;

  p1 = robot[whichRobot]->getX();
  p2 = robot[whichRobot]->getY();
  direction = robot[whichRobot]->direction;
  if(robot[whichRobot]->isaGripperRobot()){
    GripperRobot *rob;
    rob = (GripperRobot*)robot[whichRobot];
    if(rob->getCrashed()){
      return(1);
    }
  }
  mindist = robot[whichRobot]->getRadius();
  for(i=0;i<nrOfWalls;i++){
    dist = wall[i]->distR(p1,p2);
    if (dist < mindist)
      return 1;
  }
  for(i=0;i<nrOfObst;i++){
    dist = g_distPoint(p1,p2,obst[i]->x,obst[i]->y)-obst[0]->r;
    if (dist < mindist)
      return 1;
  }
  for(i=0;i<nrOfSObst;i++){
    if (sobst[i]->taken == 0.0){
      dist = g_distPoint(p1,p2,sobst[i]->x,sobst[i]->y)-sobst[0]->r;
      if (dist < mindist)
	return 1;
    }
  }
  for(i=0; i < nrOfRobots; i++){
    dist = g_distPoint(p1, p2, robot[i]->getX(), robot[i]->getY()) - robot[i]->getRadius();
    if(dist < mindist && i!=whichRobot)
      return 1;
  }
  
 
  return(0);
}

/**
 * Reset khepera position. Random new position or according to start position. Clears gripper crash status.
 * @param robotNr Which robot to reset.
 * @see loadParams()
 */
void Environ::resetRobot(int robotNr){

  if (testSameStartPosition > 0){
    if(nrOfStartPositions > 0 && nrOfStartPositions > robotNr){
      robot[robotNr]->setX(start[robotNr][0]);
      robot[robotNr]->setY(start[robotNr][1]);
      robot[robotNr]->direction = start[robotNr][2];
    }
    else{
      fprintf(stderr,"You havent specified enough startpositions for\
testSameStartPositions in your world file\n");
      exit(-1);
    }
  }
  else{
    /* Make sure so that the gripper isn't causing us any 
     * trouble.
     **/ 
    if(robot[robotNr]->isaGripperRobot()){
      GripperRobot *bot;
      bot = (GripperRobot*)robot[robotNr];
      bot->clearCrashed();
    }
    robot[robotNr]->setX(g_rans(dx));
    robot[robotNr]->setY(g_rans(dy));
    robot[robotNr]->direction = fabs(g_rans(360.0));   
    while (checkCrash(robotNr) == 1 ){
      robot[robotNr]->setX(g_rans(dx));
      robot[robotNr]->setY(g_rans(dy));
    }	   
  }
}
/**
 * Prints the input array of a robot and a user added message to stdout.
 * @param robotNr Input for which robot.
 * @param str User string to print.
 */
void Environ::printInput(int robotNr, const char *str) const{
  int i;

  printf("----ROBOT %d input; START -> %s\n",robotNr,str);
  for(i=0; i < NETWORK_INPUTS; i++)
    printf("%.2f ",input[robotNr][i]);
  printf("\n");
  printf("----ROBOT %d input; END -> %s\n",robotNr,str);
}

/**
 * Adds the inputs of \a sens to a robots infrared sensors. All according to nrOfFrontInfrared.
 * @param robotNr For which robot.
 * @param sens Array of 8 floats.
 */
void Environ::addToInputArray(int robotNr, float *sens){
  int ii=0;
  switch(nrOfFrontInfrared){
  case 6:
    for(ii=0;ii<8;ii++){
      input[robotNr][ii] += sens[ii];
    }
    break;
  case 4:
    input[robotNr][0] += (sens[0] + sens[1]) / 2;
    input[robotNr][1] += (sens[1] + sens[2]) / 2;
    input[robotNr][2] += (sens[3] + sens[4]) / 2;
    input[robotNr][3] += (sens[4] + sens[5]) / 2;
    input[robotNr][6] += sens[6];
    input[robotNr][7] += sens[7];
    break;
  case 2:
    input[robotNr][0] += (sens[0] + sens[1] + sens[2]) / 3;
    input[robotNr][1] += (sens[3] + sens[4] + sens[5]) / 3;
    input[robotNr][6] += sens[6];
    input[robotNr][7] += sens[7];
    break;
  default:
    printf("Sane check in sim.c removed and Environ::addToInputArray freaked out\n");
    printf("OR\nSomething have messed up the variable nrOfFrontInfrared\n");
    printf("- nrOfFrontInfrared must be 2, 4 or 6 -\n");
    printf("BAILING OUT !!! \n");
    exit(-1);
  }
}

/**
 * Set input array for a robot. Uses global parameters loaded from parameter file. Creates a identical copy in simoutput[][].
 * @param robotNr Which robot to set input for.
 */

void Environ::setInput(int robotNr){
  int i,ii,l;
  float   groundValue=0.0;
  float p1,p2;
  double direction;
  float *sens;
  float radius;

  p1 = robot[robotNr]->getX();
  p2 = robot[robotNr]->getY();
  direction = robot[robotNr]->direction;
  radius = robot[robotNr]->getRadius();
#ifdef ENVIRON_DEBUG
  cout << "Environ::setInput(" << robotNr <<")" << endl;
  cout << "the Robot is at position " << p1 << "x, " << p2 << "y, heading " << direction << endl;
  cout << " * Sensors checking " << nrOfWalls <<" walls" << endl;
#endif
  /*
   * activate wall sensors
   */
  for(i=0;i<NETWORK_INPUTS;i++)
    input[robotNr][i] = 0.0;

  for(i=0;i<nrOfWalls;i++){
    sens=NULL;
    sens = wall[i]->sensorVal(p1,p2,(float)direction,radius);
    if(sens != NULL){
      addToInputArray(robotNr,sens);
    }
  } 
  //  printInput(0);



#ifdef ENVIRON_DEBUG
  cout << " * - Wall checking done" << endl;
  cout << " * Sensors checking " << nrOfObst <<" round obstacles" << endl;
#endif
  /*
   * activate robot -> sensors
   */
  if(nrOfRobots>1){
    for(i=0;i<nrOfRobots;i++){
      sens=NULL;
      if(i!=robotNr)
	sens = robot[i]->sensorVal(p1,p2,(float)direction,radius);
      if(sens != NULL){
	addToInputArray(robotNr,sens);
      }
    } 
  }

  
  /*
   * activate sensors from round obstacles
   */
  for(i=0;i<nrOfObst;i++){
    sens=NULL;
    sens = obst[i]->sensorVal(p1,p2,direction,radius);
    if(sens!=NULL){
      addToInputArray(robotNr,sens);
    }
  }

#ifdef ENVIRON_DEBUG
  cout << " * - Round obstacles checking done" << endl;
  cout << " * Sensors checking " << nrOfSObst <<" small round obstacles" << endl;
#endif
  /*
   * activate sensors from small round obstacles if not taken
   */   
  for(i=0;i<nrOfSObst;i++){
    sens=NULL;
    sens = sobst[i]->sensorVal(p1,p2,direction,radius);
    if(sens!=NULL){
      addToInputArray(robotNr,sens);
    } 
  }

#ifdef ENVIRON_DEBUG
  cout << " * - Small round obstacles checking done" << endl;
  if(sensorNoise >0 )
    cout << " * Sensors adding noise " << endl;
#endif


  /*
   * add noise to infra-red sensors if requested
   * if ob_taken == 1 extra noise is present
   */
  if (sensorNoise > 0){
    for(i=0; i< 8; i++){
      if (gripperSensor == 1 /* && robot[robotNr]->gripper.ob_taken == 1*/) 
	input[robotNr][i] += g_rans(amountOfNoise + 0.0);
      else
	input[robotNr][i] += g_rans(amountOfNoise);	
    }	
  }
  //printInput(0);

  for(ii=0;ii<8;ii++){
    if (input[robotNr][ii] > 1.0)
      input[robotNr][ii] = 1.0;
    if(input[robotNr][ii] < 0.0 )
      input[robotNr][ii] = 0.0;
  }


  /*
   * if we have a sensor of energy we activate it
   */ 
  if (useEnergy == 1){
    input[robotNr][EnergySensor] = robot[robotNr]->energy / 300.0;
    if (input[robotNr][EnergySensor] > 1.0) 
      input[robotNr][EnergySensor] = 1.0;
    else if(input[robotNr][EnergySensor] < 0.0)
      input[robotNr][EnergySensor] = 0.0;
  }   
  //printInput(0);
  /*
   * if we have a ground sensor we activate it
   */ 
  if (useGround == 1){
    groundValue = (float)inZone(robotNr);
    if(groundValue<0){
      input[robotNr][GroundSensor] = 0.0;
    }
    else{
      input[robotNr][GroundSensor] = 1.0;
    }
  }         

  /*
   * if we have a compass sensor we activate it
   */ 
  if (useCompass == 1){
    input[robotNr][CompassSensor] = direction / 360.0;
  }               
  /*
   * activate bulblight sensors  
   */
  if (useLightbulbSensors > 0){ 
    for(i=LightBulbSensor;i<LightBulbSensor+8;i++)
      input[robotNr][i] = 0.0;

    for(i=0;i<nrOfLights;i++){
      sens=NULL;
      sens = light[i]->sensorVal(p1,p2,direction,radius);
      if(sens!=NULL){
	for(ii=LightBulbSensor,l=0;ii< LightBulbSensor + 8; ii++,l++){
	  input[robotNr][ii] += sens[l];
	  if (input[robotNr][ii] > 1.0)
	    input[robotNr][ii] = 1.0;
	}
      }
    } 
    //    printInput(0);
    /*
     * add noise to light sensors if requested
     */ 
    if (sensorNoise == 1){ 
      for(i=LightBulbSensor; i< LightBulbSensor + 8; i++){
	if (input[robotNr][i] > 0.1)
	  input[robotNr][i] += g_rans(input[robotNr][i]);
	else
	  input[robotNr][i]+= g_rans(0.1); 
      if (input[robotNr][i] < 0.0) input[robotNr][i]=0.0;
      if (input[robotNr][i] > 1.0) input[robotNr][i]=1.0;
      }
    }
  }
  
  /*
   * activate gripper sensor
   * No simulation of resistance
   * 
   * Comment stolen from Nolfi's simulator
   * sensore di oggetto della pinza
   */
  if(gripperSensor > 0){
    if(robot[robotNr]->isaGripperRobot()){
      GripperRobot *bot;
      bot = (GripperRobot*)robot[robotNr];
      input[robotNr][GripperSensor] = bot->ob_sensed;
    }
  }

  /* N */ 
  /*
   * add input from simulated rodcamera
   */
  if(useRodSensor > 0){
    p1 = robot[robotNr]->getX();         
    p2 = robot[robotNr]->getY();          
    direction = robot[robotNr]->direction;
    radius = robot[robotNr]->getRadius(); 
    
    for(i=0; i<nrOfRobots; i++){                                    
      if(i!=robotNr){                                               
        if(robot[i]->nRod>0){                                       
          for(int cRod=0; cRod<robot[i]->nRod; cRod++){             
	    int ra,ka,oa,angle;
	    double oad,ox,oy;
	    // Get X and Y position of Rod
            float x_pos = robot[i]->getX() + 
	      robot[i]->getRodX(cRod)*cos(g_DGRtoRAD(robot[i]->direction));
            float y_pos = robot[i]->getY() + 
	      robot[i]->getRodY(cRod)*sin(g_DGRtoRAD(robot[i]->direction)); 
	    // Calculate the distance
	     float dist = g_distPoint(p1,p2,x_pos,y_pos);           

	    /* Fix Johan */
	    
	    ox = (double)(x_pos - p1);
	    oy = (double)(y_pos - p2);
	    oad = atan2(ox,oy);
	    oa =(int)g_RADtoDGR(oad);
	    /* coordinate system is upside down */
	    oa = oa * -1;
	    
	    /* rotate kangle to match oa */
	    /* coordinate system is upside down */
	    ka = g_adjustAngle360((int)direction,270);
	    ka = g_splitAngle180(ka);
	    
	    ra =(int)(oa - ka);
	    ra = g_modulo(ra,360);   

	    angle = ra;
            angle = g_modulo(angle+ROD_ANGLE/2, 360);          // To be able to use the angle range [0..CAMERA_ANGLE]
	    // end fix Johan

            if((dist>=0) && (dist<ROD_MAX_DISTANCE) && (angle>=0) && (angle<=ROD_ANGLE)){   // Check if the object is in range and in front of the robot
              // Ok, the object is in range...
              int p_retina = (angle*ROD_RESOLUTION)/ROD_ANGLE;  // Calc the position on the retina
              if((p_retina>=0) && (p_retina<ROD_RESOLUTION))
                input[robotNr][RodSensor+p_retina] = 1;            // Set the corresponding camera position to 1
            }
          }
        }
      }
    }
    
    // If specified, add noise...
    if (useRodNoise == 1){ 
      for(i=RodSensor; i< RodSensor + ROD_RESOLUTION; i++){
        input[robotNr][i] += g_rans(float(amountOfNoise))-amountOfNoise/2;  // Add noise
        if (input[robotNr][i] < 0.0) input[robotNr][i]=0.0;               // Ensure input is between boundaries
        if (input[robotNr][i] > 1.0) input[robotNr][i]=1.0;               // Ensure input is between boundaries
      }
    }
  }
  /* N */
  for(i=0;i < NETWORK_INPUTS; i++){
    simoutput[robotNr][i]=input[robotNr][i];
  }
}