neuro_kick05_bms.c

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

}

//============================================================================
// NeuroKick05::getWSState(NeuroKick05State &state)
//============================================================================
void
NeuroKick05::getWSState(NeuroKick05State &state)
{
  //fill world information
  state.ivMyWorldPosition = WSinfo::me->pos;
  state.ivMyWorldVelocity = WSinfo::me->vel;
  state.ivMyWorldANGLE    = WSinfo::me->ang;
  state.ivBallWorldPosition = WSinfo::ball->pos;
  state.ivBallWorldVelocity = WSinfo::ball->vel;
  //fill opponent information
  WSpset pset= WSinfo::valid_opponents;
  pset.keep_and_sort_closest_players_to_point(1, state.ivBallWorldPosition);
  if ( pset.num )
  {
    state.ivOpponentPosition = pset[0]->pos;
    state.ivOpponentBodyAngle = pset[0]->ang;
    state.ivOpponentBodyAngleAge = pset[0]->age_ang;
  }
  else
  {
    state.ivOpponentPosition = Vector(1000,1000); // outside pitch
    state.ivOpponentBodyAngle = ANGLE(0);
    state.ivOpponentBodyAngleAge = 1000;
  }
}  

//============================================================================
// NeuroKick05::set_state()
//============================================================================
void 
NeuroKick05::set_state( const Vector &myPos, 
                        const Vector &myVel,
                        const ANGLE  &myAng,
                        const Vector &ballPos,
                        const Vector &ballVel)
{
  ivFakeState.ivMyWorldPosition     = myPos;
  ivFakeState.ivMyWorldVelocity     = myVel;
  ivFakeState.ivMyWorldANGLE        = myAng;
  ivFakeState.ivBallWorldPosition   = ballPos;
  ivFakeState.ivBallWorldVelocity   = ballVel;
  ivFakeStateTime = WSinfo::ws->time;
  ivInitInCycle = -1;
}

//============================================================================
// NeuroKick05::reset_state()
//============================================================================
void 
NeuroKick05::reset_state() 
{
  ivFakeStateTime = -1;
  ivInitInCycle   = -1;
}

//============================================================================
// NeuroKick05::getCurrentState()
//============================================================================
NeuroKick05State
NeuroKick05::getCurrentState()
{
  NeuroKick05State returnValue;
  if (ivFakeStateTime == WSinfo::ws->time) 
  {
    returnValue = ivFakeState;
  } 
  else 
  {
    getWSState(returnValue);
  }
  return returnValue;
}

//============================================================================
// NeuroKick05::isFailureState()
//============================================================================
bool             
NeuroKick05::isFailureState(const NeuroKick05State & s)
{
//cout<<"NeuroKick05: Check for failure state: s.ivRelativeBallDistance                    = "<<s.ivRelativeBallDistance<<endl;
//cout<<"                                      cvcMaximalDesiredBallPlayerDistanceRelative = "<<cvcMaximalDesiredBallPlayerDistanceRelative<<endl;
  if (s.ivRelativeBallDistance > cvcMaximalDesiredBallPlayerDistanceRelative)
    return true; //ball too near to end of kickable area
  if (s.ivRelativeBallDistance < cvcMinimalDesiredBallPlayerDistanceRelative)
    return true; //ball too near to me (danger of collision) 
  return false; //no failure
}

//============================================================================
// NeuroKick05::setNeuralNetInput( )
//============================================================================
void             
NeuroKick05::setNeuralNetInput( NeuroKick05State       & state,
                                Value                    targetVelocity,
                                ANGLE                    targetDir,
                                float                  * net_in )
{
  //NOTE: targetDir and targetVelocity are disregarded, 
  //      these parameters are considered for downward compatibility only
  
  //set neural net input
  net_in[0] = state.ivBallPositionRelativeAngle / 180.0;
  net_in[1] = state.ivRelativeBallDistance;
  net_in[2] = state.ivRelativeBallVelocity.x;
  net_in[3] = state.ivRelativeBallVelocity.y;
  net_in[4] = state.ivRelativeBodyToKickAngle / 180.0;
}

//============================================================================
// NeuroKick05::chooseCurrentNet( )
//============================================================================
Net * 
NeuroKick05::chooseCurrentNet()
{
  if (ivTargetKickVelocity > 2.3) return cvpNetArray[0]; //max speed
  if (ivTargetKickVelocity > 2.0) return cvpNetArray[1]; //high speed
  if (ivTargetKickVelocity > 1.5) return cvpNetArray[2]; //average speed
  return cvpNetArray[3];                                 //slow speed
}

//============================================================================
// NeuroKick05::evaluateState()
//============================================================================
Value            
NeuroKick05::evaluateState( NeuroKick05State & state)
{
  //set the features, do the rotation
  this->completeState( state );

  //lowest possible value is state is a failure state
  //(considering cases where the ball goes outside the kickable area
  //or collides with the player)
  if (isFailureState(state))
  {
//cout<<"NeuroKick05: Considered state is a failure state!"<<endl;
    return 0.0;
  }
  //check whether oneortwostepkick behavior considers this state ok
  //(takes opponents into consideration)
  MyState dummy;
  dummy.my_pos = state.ivMyWorldPosition - state.ivMyWorldVelocity;
  dummy.my_vel = state.ivMyWorldVelocity;
  dummy.op_pos = state.ivOpponentPosition;
  dummy.op_bodydir = state.ivOpponentBodyAngle;
  dummy.op_bodydir_age = state.ivOpponentBodyAngleAge;
  if ( ! ivpOneStepKickBehavior->is_pos_ok( dummy, 
                                            state.ivBallWorldPosition ) )
  {
    //cout<<"NeuroKick05: Future position "<<state.ivMyWorldPosition<<" and ball@"<<state.ivBallWorldPosition<<" is not ok (says OneStepKick)!"<<endl;
    return 0.0;
  } 
  //ask the net
  this->setNeuralNetInput( state,
                           ivTargetKickVelocity,
                           ivTargetKickAngle,
                           ivpCurrentNet->in_vec );
  ivpCurrentNet->forward_pass( ivpCurrentNet->in_vec,
                               ivpCurrentNet->out_vec );
  return ivpCurrentNet->out_vec[0];
}

//============================================================================
// NeuroKick05::decideForAnAction()
//============================================================================
bool
NeuroKick05::decideForAnAction(Cmd & cmd)
{
  NeuroKick05State currentState = this->getCurrentState();
  NeuroKick05State successorState, bestSuccessorState;
  Cmd_Main bestAction;
  bool actionFound = false;
  Value bestSuccessorStateValue = -1.0;
  Value bestKickPower = 0.0;
  Angle bestKickAngle = 0.0;
  
  //try to use oneortwostepkick behavior, if possible
  if ( 1 ) //just in case we would like to switch of the use of 12step kick
  {
    Value resultingVelocityAfter1StepKick,
          resultingVelocityAfter2StepKick;
    Cmd   cmdForOneStep, cmdForTwoStep;
    //try oneortwostep kick
    ivpOneOrTwoStepKickBehavior->set_state( currentState.ivMyWorldPosition,
                                            currentState.ivMyWorldVelocity,
                                            currentState.ivMyWorldANGLE,
                                            currentState.ivBallWorldPosition,
                                            currentState.ivBallWorldVelocity,
                                            currentState.ivOpponentPosition,
                                            currentState.ivOpponentBodyAngle,
                                            currentState.ivOpponentBodyAngleAge);
    if (ivDoTargetTracking)
    {
      ivpOneOrTwoStepKickBehavior->kick_to_pos_with_initial_vel(
                                            ivTargetKickVelocity,
                                            ivTargetKickPosition );
    }
    else
    {
      ivpOneOrTwoStepKickBehavior->kick_in_dir_with_initial_vel(
                                            ivTargetKickVelocity,
                                            ivTargetKickAngle );
    }
    ivpOneOrTwoStepKickBehavior->get_cmd( cmdForOneStep,
                                          cmdForTwoStep );
    ivpOneOrTwoStepKickBehavior->get_vel( resultingVelocityAfter1StepKick,
                                          resultingVelocityAfter2StepKick );
    //let us decide if the quality of the kick, which can be obtained via
    //a oneortwostepkick, is sufficient for us
    if (   fabs(resultingVelocityAfter1StepKick - ivTargetKickVelocity)
         < cvMaximalKickVelocityDeviation  )
    {
      cmd.cmd_main.clone( cmdForOneStep.cmd_main );
      //cout<<"NeuroKick05: MAKE ONE STEP KICK!"<<endl;
      return true;
    }
    if (   fabs(resultingVelocityAfter2StepKick - ivTargetKickVelocity)
         < 0.85 * cvMaximalKickVelocityDeviation  )
    {
      cmd.cmd_main.clone( cmdForTwoStep.cmd_main );
      //cout<<"NeuroKick05: MAKE TWO STEP KICK!"<<endl;
      return true;
    }
  }

  //cout<<"NeuroKick05: MAKE A REAL   N E U R O   KICK!"<<endl;

  //so, a oneortwostepkick will not suffice, we need a real neuro kick
  ivpCurrentNet = chooseCurrentNet();
  for (int fineTuning=0; fineTuning<2; fineTuning++)
  {
    if ( fineTuning == 0 )
      ivActionInterator.reset(false);
    else
      ivActionInterator.reset(true, bestKickPower, ANGLE(bestKickAngle) );
    //loop over all actions
    while (Cmd_Main const * chosenAction = ivActionInterator.next() )  
    {
      //cpmpute successor state
      Tools::model_cmd_main( currentState.ivMyWorldPosition,
                             currentState.ivMyWorldVelocity,
                             currentState.ivMyWorldANGLE,
                             currentState.ivBallWorldPosition,
                             currentState.ivBallWorldVelocity,
                             * chosenAction,
                             successorState.ivMyWorldPosition,
                             successorState.ivMyWorldVelocity,
                             successorState.ivMyWorldANGLE,
                             successorState.ivBallWorldPosition,
                             successorState.ivBallWorldVelocity );
      //evaluate the successor state
      Value successorStateValue = this->evaluateState( successorState );
      //if (fineTuning==0) {cout<<"NeuroKick05: ITERATION, aktuell: "<<*chosenAction<<" erbringt "<<successorStateValue;
      //if (isFailureState(successorState)) cout<<" (failure)"; cout<<endl;}
      if (    bestSuccessorStateValue < 0.0
           || actionFound == false 
           || bestSuccessorStateValue < successorStateValue )
      {
        bestSuccessorStateValue = successorStateValue;
        bestSuccessorState = successorState;
        bestAction = * chosenAction;
        bestAction.get_kick( bestKickPower,
                             bestKickAngle );
        actionFound = true;
      }
    } //end of while
    if ( ! ivDoFineTuning || actionFound == false )
      break;
  } //end of for loop
  if (actionFound)
  {
    //cout<<"NeuroKick05: Chosen Action: "<<bestAction<<" with value="<<bestSuccessorStateValue<<endl;

    return cmd.cmd_main.clone( bestAction );
  }
  return false;
}