neuro_kick05_bms.c

Brainstormers(头脑风暴)队是05年robocup冠军,这是05年Brainstormers公布的源代码,Brainstormers是robocup老牌的德国强队

/*
Brainstormers 2D (Soccer Simulation League 2D)
PUBLIC SOURCE CODE RELEASE 2005
Copyright (C) 1998-2005 Neuroinformatics Group,
                        University of Osnabrueck, Germany

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 (at your option) 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#include "neuro_kick05_bms.h"
#define BASELEVEL 0

//============================================================================
//DECLARATION OF STATIC VARIABLES
//============================================================================
bool    NeuroKick05::cvInitialized = false;;
Value   NeuroKick05::cvMaximalKickVelocityDeviation = 0.1;
Value   NeuroKick05::cvMaximalKickDirectionDeviation= DEG2RAD(10);
Net   * NeuroKick05::cvpNetArray[NUMBER_OF_NEURAL_NETS];

//============================================================================
// NeuroKick05::init()
//============================================================================
bool NeuroKick05::init ( char const * confFile, 
                         int          argc, 
                         char const* const* argv) 
{
  if(cvInitialized) 
    return true;
    
  if ( ! OneOrTwoStepKick::init(confFile,argc,argv) ) 
  {
    ERROR_OUT 
    << "\nCould not initialize OneOrTwoStepKick behavior - stop loading.";
    exit(1);
  }
  if ( ! OneStepKick::init(confFile,argc,argv) ) 
  {
    ERROR_OUT 
    << "\nCould not initialize OneStepKick behavior - stop loading.";
    exit(1);
  }

  //currently at most 2 nets are used
  for (int i=0; i<NUMBER_OF_NEURAL_NETS; i++)
    cvpNetArray[i] = new Net();
  
  char NN0_name[500];
  char NN1_name[500];
  char NN2_name[500];
  char NN3_name[500];
  sprintf(NN0_name,"%s","./data/nets_neuro_kick05/kickLearn_maxspeednet27.net\0"); //maxspeed
  sprintf(NN1_name,"%s","./data/nets_neuro_kick05/kickLearn_highspeednet25.net\0");//highspeed
  sprintf(NN2_name,"%s","./data/nets_neuro_kick05/kickLearn_averagespeednet20.net\0");//averagespeed
  sprintf(NN3_name,"%s","./data/nets_neuro_kick05/kickLearn_slowspeednet15.net\0");//slowspeed
  
  ValueParser vp(confFile,"Neuro_Kick05");
  //vp.set_verbose();
  vp.get("maximal_kick_velocity_deviation",cvMaximalKickVelocityDeviation);
  vp.get("maximal_kick_direction_deviation",cvMaximalKickDirectionDeviation);
  
  vp.get("NN0_name", NN0_name, 500);
  vp.get("NN1_name", NN1_name, 500);
  vp.get("NN2_name", NN2_name, 500);
  vp.get("NN3_name", NN3_name, 500);
  
  if (cvpNetArray[0]->load_net(NN0_name) == FILE_ERROR) 
  {
    ERROR_OUT<<"NeuroKick_bms: No net0 "<<NN0_name<<" found - stop loading\n";
    exit(0);
  }
  if (cvpNetArray[1]->load_net(NN1_name) == FILE_ERROR) 
  {
    ERROR_OUT<<"NeuroKick_bms: No net1 "<<NN1_name<<" found - stop loading\n";
    exit(0);
  }
  if (cvpNetArray[2]->load_net(NN2_name) == FILE_ERROR) 
  {
    ERROR_OUT<<"NeuroKick_bms: No net2 "<<NN2_name<<" found - stop loading\n";
    exit(0);
  }
  if (cvpNetArray[3]->load_net(NN3_name) == FILE_ERROR) 
  {
    ERROR_OUT<<"NeuroKick_bms: No net3 "<<NN3_name<<" found - stop loading\n";
    exit(0);
  }
  
  cout<<"\nNeuroKick05 behavior initialized.";
  cvInitialized = true;
  return true;
}

//============================================================================
// CONSTRUCTOR
// NeuroKick05::NeuroKick05()
//============================================================================
NeuroKick05::NeuroKick05()
{
  ivDoFineTuning = false;
  ivInitInCycle = -1;
  ivFakeStateTime = -1;
  ivpOneOrTwoStepKickBehavior = new OneOrTwoStepKick();
  ivpOneStepKickBehavior      = new OneStepKick();
}

//============================================================================
// DESTRUCTOR
// NeuroKick05::~NeuroKick05()
//============================================================================
NeuroKick05::~NeuroKick05()
{
  for (int i=0; i<NUMBER_OF_NEURAL_NETS; i++)
    if (cvpNetArray[i]) delete cvpNetArray[i];
  if (ivpOneOrTwoStepKickBehavior)
    delete ivpOneOrTwoStepKickBehavior;
  if (ivpOneStepKickBehavior)
    delete ivpOneStepKickBehavior;
}

//============================================================================
// SKILL'S MAIN METHOD
// NeuroKick05::get_cmd()
//============================================================================
bool
NeuroKick05::get_cmd(Cmd & cmd)
{
  if ( !cvInitialized ) 
  {
    ERROR_OUT << "\nNeuroKick05 not initialized!";
    return false;
  }
  if ( WSinfo::ws->time != ivInitInCycle) 
  {
    ERROR_OUT << "\nNeuroKick05::get_cmd() called without prior initialization!";
    return false;
  }
  LOG_MOV(0,<<"Starting NeuroKick05 behavior (dir: "<< ivTargetKickAngle.get_value()
    <<", vel: "<<ivTargetKickVelocity<<").");
  //cout<<"NeuroKick05: Starting NeuroKick05 behavior (dir: "<< ivTargetKickAngle.get_value()
  //  <<", vel: "<<ivTargetKickVelocity<<")."<<endl;
  return decideForAnAction(cmd);
}

//============================================================================
// SKILL'S INITIALIZATION METHOD
// NeuroKick05::kick_to_pos_with_initial_vel()
//============================================================================
void 
NeuroKick05::kick_to_pos_with_initial_vel  (  Value          vel,
                                              const Vector & pos) 
{
  NeuroKick05State state = getCurrentState();
  ivTargetKickAngle    = (pos - state.ivBallWorldPosition).ARG();
  ivTargetKickVelocity = vel;
  ivTargetKickPosition = pos;
  ivDoTargetTracking   = true;
  ivInitInCycle = WSinfo::ws->time;
}

//============================================================================
// SKILL'S INITIALIZATION METHOD
// NeuroKick05::kick_to_pos_with_final_vel()
//============================================================================
void 
NeuroKick05::kick_to_pos_with_final_vel  ( Value         vel, 
                                           const Vector &pos )
{
  NeuroKick05State state = getCurrentState();
  ivTargetKickAngle    = (pos - state.ivBallWorldPosition).ARG();

  ivTargetKickVelocity = computeInitialVelocityForFinalVelocity
                           ( ( WSinfo::ball->pos - pos ).norm() ,
                             vel );
  //Does not work if target_vel > ball_speed_max... use max speed instead!
  if ( ivTargetKickVelocity > ServerOptions::ball_speed_max) 
  {
    ivTargetKickVelocity = ServerOptions::ball_speed_max;
    LOG_ERR(0,<<"NeuroKick: Point "<<pos<<" too far away, using max vel!");
  }

  ivTargetKickPosition = pos;
  ivDoTargetTracking   = true;
  ivInitInCycle = WSinfo::ws->time;
}

//============================================================================
// SKILL'S INITIALIZATION METHOD
// NeuroKick05::kick_to_pos_with_max_vel()
//============================================================================
void 
NeuroKick05::kick_to_pos_with_max_vel( const Vector &pos ) 
{
  kick_to_pos_with_initial_vel ( ServerOptions::ball_speed_max, pos );
}

//============================================================================
// SKILL'S INITIALIZATION METHOD
// NeuroKick05::kick_in_dir_with_max_vel()
//============================================================================
void NeuroKick05::kick_in_dir_with_max_vel(const ANGLE &dir ) 
{
  kick_in_dir_with_initial_vel( ServerOptions::ball_speed_max, dir );
}

//============================================================================
// SKILL'S INITIALIZATION METHOD
// NeuroKick05::kick_in_dir_with_initial_vel()
//============================================================================
void NeuroKick05::kick_in_dir_with_initial_vel( Value vel,
                                                const ANGLE &dir ) 
{
  ivTargetKickAngle    = dir;
  ivTargetKickVelocity = vel;
  ivDoTargetTracking   = false;
  ivInitInCycle = WSinfo::ws->time;
}

//============================================================================
// HELPER METHOD FOR INITIALIZATION
// NeuroKick05::computeInitialVelocityForFinalVelocity()
//============================================================================
Value
NeuroKick05::computeInitialVelocityForFinalVelocity( Value  distance,
                                                     Value  finalVelocity)
{
  Value  remainingDistance = distance;
  Value  currentVelocity   = finalVelocity;
  while (remainingDistance > 0.0)
  {
    currentVelocity   /= ServerOptions::ball_decay;
    remainingDistance -= currentVelocity;
  }
  return currentVelocity;
}

//============================================================================
// NeuroKick05::completeState
//============================================================================
void 
NeuroKick05::completeState(NeuroKick05State &state)
{
  //fill elements for neural net input
  ANGLE tmpAngle = (state.ivBallWorldPosition - state.ivMyWorldPosition).ARG();
  tmpAngle -= state.ivMyWorldANGLE;
  state.ivBallPositionRelativeAngle 
    = ( ( tmpAngle ).get_value_mPI_pPI()) * (180.0/PI);  //value in [-180,180]
float v1 = ( (state.ivBallWorldPosition - state.ivMyWorldPosition).norm()
          - ServerOptions::ball_size
          - ServerOptions::player_size);
float v2 = ( WSinfo::me->kick_radius 
          - ServerOptions::ball_size 
          - ServerOptions::player_size);
  state.ivRelativeBallDistance      
    = v1
      /
      v2  ;
  //cout<<"TESTAUSGABE: b:"<<state.ivBallWorldPosition<<" p:"<<state.ivMyWorldPosition<<" rel:"<<state.ivRelativeBallDistance<<endl;
  //cout<<"TESTAUSGABE: v1:"<<v1<<" v2:"<<v2<<" kr:"<<WSinfo::me->kick_radius<<" bs:"<<ServerOptions::ball_size<<" ps:"<<ServerOptions::player_size<<endl;
  state.ivRelativeBallVelocity      = state.ivBallWorldVelocity
                                      - state.ivMyWorldVelocity;
  tmpAngle = state.ivMyWorldANGLE;
  tmpAngle -= ivTargetKickAngle;
  state.ivRelativeBodyToKickAngle   = (  tmpAngle ).get_value_mPI_pPI()
                                      * (180.0/PI); //value in [-180,180]
  //now do the rotation according to the kick direction
  //note: we do not have to rotate the ball distance to the player as well
  //      as the relative angular distance between ball and player orientation
  state.ivRelativeBallVelocity.ROTATE( ANGLE(2.0*PI) - ivTargetKickAngle );
  //state.ivRelativeBodyToKickAngle ==> no rotation necessary, this has been
  //  realized above already (by subtracting ivTargetKickAngle!