environ.cc

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

/*
  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.

 */

#include "environ.h"
/**
 * @brief Constructor for a Environ (environment). 
 * Initiates a new world with \a paramNrOfRobots which is set by
 * a call to loadParams(). It also initiates the geometric library
 * by calling gemometricInit(). At last the newly created robots
 * are initiated by calling Robot::init().
 * @attention loadParams() must be called before creating a new Environ.
 * @see loadParams()
 * @see geometricInit()
 * @see Robot::init()
 */
Environ::Environ(){
  char   tmpStr[300];
  nrOfLights = 0;
  nrOfWalls = 0;
  nrOfObst = 0;
  nrOfSObst = 0;
  nrOfRobots = 0;
  nrOfZones = 0;
  wall = NULL;
  robot = NULL;
  start = NULL;
  obst = NULL;
  sobst = NULL;
  zone = NULL;
  light = NULL;
  dx = 0;
  dy = 0;
  geometricInit();
  robot =  new Robot*[paramNrOfRobots];
  for( int i=0; i < paramNrOfRobots; i++){
   robot[i] =  new GripperRobot();
   
    nrOfRobots++;
    /* HACK */
    robot[i]->addRod(0,0);
    /* SLUT */
  }
  if(paramNrOfRobots>1){
    sprintf(tmpStr,"%s/%s",cameraPath,robotCamera);
    robot[0]->init(tmpStr);
  }
}

/**
 * Resets all robots in the world.
 * @see Robot::resetRobot()
 */
void Environ::initWorld(){
  for(int i=0; i < nrOfRobots; i++)
    robot[i]->resetRobot();
}
/**
 *Deconstructor, deallocates all memory allocated.
 *
 */
Environ::~Environ(){
  int i;
  if(nrOfLights > 0){
    for(i=0; i < nrOfLights; i++){
      delete(light[i]);
    }
    free(light);
  }
  if(nrOfWalls > 0){
    for(i=0;i < nrOfWalls; i++){
      delete(wall[i]);
    }
    free(wall);
  }
  if(nrOfObst>0){
    for(i=0; i < nrOfObst; i++){
      delete(obst[i]);
    }
    free(obst);
  }

  if(nrOfSObst > 0){
    for(i=0; i < nrOfSObst; i++){
      delete(sobst[i]);
    }
    free(sobst);
  }
  if(nrOfRobots > 0){
    for(i=0; i < nrOfRobots; i++){
      delete(robot[i]);
    }
    free(robot);
  }
  if( nrOfZones > 0){
    for(i=0; i < nrOfZones; i++){
      delete(zone[i]);
    }
    free(zone);
  }
  
}

/*
 * Run a robot in the environment one timestep (100 ms)
 * @param o1 First argument for Robot::run() 
 * @param o2 Second argument for Robot::run()
 * @param robotNr Which robot to run.
 */

void Environ::runStep(float o1, float o2, int robotNr){
  robot[robotNr]->run(o1,o2);
}

/**
 * Get the X coordinate of Zone \a index.
 * @param index the internal index of the zone.
 * @return The x coordinate of the zone.
 */
float Environ::getZoneX(int index) const{
  return(zone[index]->x);
}
/**
 * Get the y coordinate of Zone \a index.
 * @param index the internal index of the zone.
 * @return The y coordinate of the zone.
 */
float Environ::getZoneY(int index) const{
  return(zone[index]->y);
}

/**
 * Set taken attribute on small round obstacle sobst::taken
 * @param index The internal index of the obstacle.
 */
void Environ::setTaken(int index){
  sobst[index]->taken=1.0;
}


#ifdef GUI
/**
 * Draw all robots to the screen
 * @attention This method is only available when \a GUI is defined
 */
void Environ::drawBots(){
  Robot *rob;
  GripperRobot *grob;
  int ldir,rdir;
  int a[10];
  int b[10];
  
  if(gfxUpdate){
    gui_drawBG();
    for(int robotNr=0; robotNr < nrOfRobots; robotNr++){
      
      rob = robot[robotNr];

      gui_updateSensorWindow(robotNr);      
      if(notTrajactoryRobot){
	gui_drawRobot((int)(xScale*rob->getX()),
		      (int)(xScale*rob->getY()),
		      rob->direction,
		      (int)(xScale*rob->getRadius()),
		      1,robotNr);
	if(rob->isaGripperRobot()){
	  grob = (GripperRobot*) rob;
	  for(int i=0; i < 4; i++){
	    a[i] = (int)(xScale*grob->lgripper[i]);
	    b[i] = (int)(xScale*grob->rgripper[i]);
	  }
	  
	  ldir = g_controlAngle((int)(rob->direction + 90));
	  rdir = g_controlAngle((int)(rob->direction - 90));
	  b[4] = (int)(xScale * grob->flx);
	  b[5] = (int)(xScale * grob->fly);
	  b[6] = (int)(xScale * g_displaceX(grob->gx,
					    77.0/2.0,
					    ldir));
	  b[7] = (int)(xScale * g_displaceY(grob->gy,
					    77.0/2.0,
					    ldir));

	  a[4] = (int)(xScale * grob->frx);
	  a[5] = (int)(xScale * grob->fry);
	  a[6] = (int)(xScale * g_displaceX(grob->gx,
					    77.0/2.0,
					    rdir));
	  a[7] = (int)(xScale * g_displaceY(grob->gy,
					    77.0/2.0,
					    rdir));
	  
 	  gui_drawGripper(a,b,
			  1,
			  robotNr,
			  8
			  );
	}
      }
      else{
	gui_drawRobot((int)(xScale*rob->getX()),
		      (int)(xScale*rob->getY()),
		      rob->direction,
		      (int)(xScale*rob->getRadius()),
		      2,robotNr);
	if(rob->isaGripperRobot()){
	  grob = (GripperRobot*) rob;
	  for(int i=0; i < 4; i++){
	    a[i] = (int)(xScale*grob->lgripper[i]);
	    b[i] = (int)(xScale*grob->rgripper[i]);
	  }
	  gui_drawGripper(a,b,
			  2,
			  robotNr,
			  2);
	}
      }
    }
    if(!notTrajactoryRobot)
      gui_drawBG();

    for(int i=0; i < nrOfSObst; i++){
       if(sobst[i]->taken==0.0)
	 gui_drawSObst((int)(xScale * sobst[i]->x),
		       (int)(xScale * sobst[i]->y),
		       (int)(xScale * sobst[i]->r)
		       );

    }
    gui_redrawWorld();
  }
}
#endif /* GUI */

/**
 * Get the number of zones in the world.
 * @return The number of zones.
 */
int Environ::getNrOfZones() const{
  return(nrOfZones);
}
/**
 * Get the number of walls in the world.
 * @return The number of walls.
 */
int Environ::getNrOfWalls() const{
  return(nrOfWalls);
}
/**
 * Get the number of small round obstacles in the world.
 * @return The number of small round obstacles.
 */
int Environ::getNrOfSObst() const{
  return(nrOfSObst);
}
/**
 * Get the number of light sources in the world.
 * @return The number of light sources.
 */
int Environ::getNrOfLights() const{
  return(nrOfLights);
}

/**
 * Get the number of round obstacles in the world.
 * @return The number of round obstacles.
 */
int Environ::getNrOfObst() const {
  return(nrOfObst);
}

/**
 * Checks if robot \a rR is carrying something and if true moves that object according to the robots movement.
 * @param rR The robot.
 */
void Environ::moveSObst(int rR){
  int car;
  car = robot[rR]->carry; 
  if(car > -1)
    if(car > -1 && car < nrOfSObst)
      sobst[car]->tempMove(robot[rR]->getLastDist(),robot[rR]->getLastAngR());
}

/**
 * Simulates the gripper of a robot for 100 ms.
 * @param arm Arm position to reach [0.0, down back -  1.0, down front].
 * @param gripper Gripper position to reach [0.0 open, 1.0 close].
 * @param robotNr Which robots gripper..
 */
void Environ::runGripper(float arm, float gripper, int robotNr){
  class GripperRobot *rob;
  int obstPresent=0;
  int obstPosTaken=-1;
  rob = (class GripperRobot*)robot[robotNr];
  rob->setArmPosition(arm);
  rob->updateGripperCords();

  rob->clrGripperSensor();
  rob->clearCrashed();
  rob->updateThingInHandsCoord();
  if(rob->armDown()){
    for(int i=0; i < nrOfWalls; i++){
      if(rob->checkCrash(wall[i]->x1,wall[i]->y1,wall[i]->x2,wall[i]->y2))
	rob->armCrashed();
      
      
    }
    for(int i=0; i < nrOfObst; i++){
      if(rob->checkCrash(obst[i]))
	rob->armCrashed();
    }
    /*   check for object between hands */
    for(int i=0; i < nrOfSObst; i++){
      /*   activate sensor if object present */
      if(sobst[i]->taken == 0.0){
	switch(rob->inHands(sobst[i])){
	case GE_OBJ_PRESENT:
	  obstPresent++;
	  obstPosTaken = i;
	break;
	case GE_OBJ_CRASH:
	  rob->armCrashed();
	  break;
	default:
	  /* no operation */
	  break;
	}
      }
    }
  }
  /* Run gripper hands and detect changes */
  if(rob->setGripperPosition(gripper)){
    if(rob->handsClosed()){
      if(obstPresent){
	if(!rob->getCrashed())
	  rob->setThingInHands(sobst[obstPosTaken],
			       obstPosTaken);
      }
    }else{
      if(rob->holdingSomething()){
	rob->releaseThingInHands();
      }
    }
  }
  rob->getSane();
}

/**
 * Reset the environment, resets robots, marks objects not taken moves obstacled if specified.
 * @see resetRobot()
 * @see resetObstacle()
 */
void Environ::resetEnvironment(){
  int i;
  /* Relocate robots, random or fixed position */
  for(i=0; i < nrOfRobots; i++){
    resetRobot(i);
  }
  /* Mark all small round obstacles not taken */
  /* and move them if moveObstacles=1  */
  for(i=0; i < nrOfSObst; i++){
    resetObstacle(i); 
  }
#ifdef GUI
  if(gfxUpdate) /* show environment after reset */
    redrawWorld();
#endif /* GUI */
}

#ifdef GUI
/**
 * Draws the world to the gui back ground.
 */
void Environ::redrawWorld(){
  gui_clear();

  for(int i=0; i <nrOfWalls; i++){
    gui_drawWall((int)(xScale*wall[i]->x1),
	       (int)(xScale*wall[i]->y1),
	       (int)(xScale*wall[i]->x2),
	       (int)(xScale*wall[i]->y2));
  }
  for(int i=0; i < nrOfSObst; i++){
    gui_drawSObst((int)(xScale*sobst[i]->x),
		(int)(xScale*sobst[i]->y),
		(int)(xScale*sobst[i]->r));
  }
  for(int i=0; i < nrOfObst; i++){
    gui_drawObst((int)(xScale*obst[i]->x),
	       (int)(xScale*obst[i]->y),
	       (int)(xScale*obst[i]->r));
  }
  for(int i=0; i < nrOfZones; i++){
    gui_drawZone((int)(xScale*zone[i]->x),(int)(xScale*zone[i]->y),(int)(xScale*zone[i]->r));
  }
  for(int i=0; i < nrOfLights; i++){
    gui_drawLight((int)(xScale*light[i]->x),(int)(xScale*light[i]->y),(int)(xScale*light[i]->r));
  }
  gui_drawBG();
  gui_redrawWorld();
}
#endif /* GUI */

/**
 * Check if robot \a robotNr is in zone \a index.
 * @param robotNr Which robot.
 * @param index Internal index of the zone.
 * @return Returns 1 if in a zone otherwise 0, -1 if indexes is out of range.
 */
int Environ::inZoneI(int robotNr, int index) const{
  float rx,ry;
  if(robotNr<paramNrOfRobots && index < nrOfZones){
    rx=robot[robotNr]->getX();
    ry=robot[robotNr]->getY();
    if(zone[index]->inZone(rx,ry,robotNr)){
      return 1;
    }
    return 0;
  }else{
    return -1;
  }
}

/**