basicagent.cpp

robocup 3d, a 3d base team similar to UvA 2d

/*
*********************************************************************************
*                            In the name of Almighty                            *
*                                                                               *
*          BasicAgent.cpp : Robocup 3D Soccer Simulation Team Zigorat           *
*                     (This team was previously named Gcyrus)                   *
*                                                                               *
*  Date: 03/20/2007                                                             *
*  Author: Mahdi Hamdarsi                                                       *
*  Comments: This file contains class definition BasicAgent which contains      *
*            several routines needed for thinkin' module                        *
*                                                                               *
*********************************************************************************
*/

/*! \file BasicAgent.cpp
<pre>
<b>File:</b>          BasicAgent.cpp
<b>Project:</b>       Robocup Soccer Simulation Team: Zigorat
<b>Authors:</b>       Mahdi Hamdarsi
<b>Created:</b>       03/20/2007
<b>Last Revision:</b> $ID$
<b>Contents:</b>      This file contains class definition BasicAgent which contains
               several routines needed for thinkin' module
<hr size=2>
<h2><b>Changes</b></h2>
<b>Date</b>             <b>Author</b>          <b>Comment</b>
03/20/2007       Mahdi           Initial version created
</pre>
*/

#include "BasicAgent.h"
#include "Logger.h"



const double static_gain = 0.06;  /*!< gain is used to normalize joint movements */
const double static_maxError = 2; /*!< Maximum error in exact joint movements */

/*! This is the constructor of BasicAgent, initilizes self
    variables, sends identification signal to the server 
    and starts the agent
    \param conn Connection to server
    \param wm WorldModel to get information about field
    \param act ActHandler instance to send messages to server
*/
BasicAgent::BasicAgent( TRoboCupConnection * conn, WorldModel * wm, ActHandler *act )
{
  connection = conn;
  WM         = wm;
  ACT        = act;

  // use the scene effector to build the agent and beam to a
  // position near the center of the playing field
  ACT->sendMessage( "(scene rsg/agent/soccerbot056.rsg)" );
}

/*! This method initializes the agent and sets up everything needed for agent to start */
void BasicAgent::InitializePlayerPosition()
{
  VecPosition posBeam = WM->getStrategicPosition();
  beam( posBeam.getX(), posBeam.getY(), posBeam.getZ() );
}

/*! This method inits the agent for server by sending his name and number */
void BasicAgent::sendPlayerInformation()
{
  char cmd[128];
  sprintf( cmd, "(init (unum %d)(teamname %s))", WM->getAgentNumber(), WM->getTeamName().c_str() );
  ACT->sendMessage( cmd );
}

/*! This method sends a beam command to server: A beam command is a command
    that moves a robot in less than a moment to the desired location (not
    in the gameplay)
    \param dx X-Coordinate of desired move location
    \param dy Y-Coordinate of desired move location
    \param dang Agent direction when has moved to the deisred location */
void BasicAgent::beam( double dx, double dy, double dang )
{
  char str[128];
  sprintf( str, "(beam %f %f %f)", dx, dy, dang );
  ACT->sendMessage( str );
}

/*! This function directly adjustes joints angular speeds by
    the values given
    \param id Joint ID to be updated
    \param dSpeed1 Joints First angular speed
    \param dSpeed2 Joints Secondangular speed
    \return bool indicating weather information were accepted or not
*/
bool BasicAgent::adjustJoint( JointT id, double dSpeed1, double dSpeed2 )
{
  if( dSpeed1 == UnknownDoubleValue || id == JID_ILLEGAL )
    return false;

  if( dSpeed2 == UnknownDoubleValue )
    dSpeed2 = 0;

  ACT->putCommandInQueue( SoccerCommand( CMD_JOINT, id, dSpeed1, dSpeed2 ) );
  return true;
}

/*--------------------------------------------------------------------------*/
/*----------------------- Changing of hinge joints -------------------------*/
/*--------------------------------------------------------------------------*/

/*! This method changes the angle of First joint of leg (Hip), if
    currently  value  is near the  current angle of  knee then no 
    changes are made.
    \param side which leg to change angle
    \param value new angle to change to
    \param dSpeed speed to set for action */
bool BasicAgent::rotateLeg( SideT side, AngDeg value, double dSpeed )
{
  if( dSpeed == -1 )
    dSpeed = static_gain;

  JointT joint = ( side == SIDE_LEFT ) ? JID_LLEG_1 : JID_RLEG_1;
  AngDeg curAngle = WM->getJoint( joint ).getAxis();

  if( fabs( curAngle - value ) > static_maxError )
  {
    adjustJoint( joint, dSpeed * (value - curAngle) );
    return false;
  }
  else
  {
    adjustJoint( joint, 0 );
    return true;
  }
}

/*! This method changes the angle of First joint of arm(Shoulder), if
    currently value is near the current angle of knee then no changes
    are made.
    \param side which leg to change angle
    \param value new angle to change to
    \param dSpeed speed to set for action */
bool BasicAgent::rotateArm( SideT side, AngDeg value, double dSpeed )
{
  if( dSpeed == -1 )
    dSpeed = static_gain;

  JointT joint = ( side == SIDE_LEFT ) ? JID_LARM_3 : JID_RARM_3;
  AngDeg curAngle = WM->getJoint( joint ).getAxis();

  if( fabs( curAngle - value ) > static_maxError )
  {
    adjustJoint( joint, dSpeed * (value - curAngle) );
    return false;
  }
  else
  {
    adjustJoint( joint, 0 );
    return true;
  }
}

/*! This method changes the angle of knee, if currently value is near
    the current angle of knee then no changes are made
    \param side which leg to change angle
    \param value new angle to change to
    \param dSpeed speed to set for action */
bool BasicAgent::knee( SideT side, AngDeg value, double dSpeed )
{
  if( dSpeed == -1 )
    dSpeed = static_gain;

  JointT joint = ( side == SIDE_LEFT ) ? kneeL : kneeR;
  AngDeg curAngle = WM->getJoint( joint ).getAxis();

  if( fabs( curAngle - value ) > static_maxError )
  {
    adjustJoint( joint, dSpeed * (value - curAngle) );
    return false;
  }
  else
  {
    adjustJoint( joint, 0 );
    return true;
  }
}

/*! This method changes the angle of knee, if currently value is near
    the current angle of knee then no changes are made
    \param side which leg to change angle
    \param value new angle to change to
    \param dSpeed speed to set for action */
bool BasicAgent::elbow( SideT side, AngDeg value, double dSpeed )
{
  if( dSpeed == -1 )
    dSpeed = static_gain;

  JointT joint = ( side == SIDE_LEFT ) ? elbowL : elbowR;
  AngDeg curAngle = WM->getJoint( joint ).getAxis();

  if( fabs( curAngle - value ) > static_maxError )
  {
    adjustJoint( joint, dSpeed * (value - curAngle) );
    return false;
  }
  else
  {
    adjustJoint( joint, 0 );
    return false;
  }
}

/*--------------------------------------------------------------------------*/
/*--------------------- Changing of universal joints -----------------------*/
/*--------------------------------------------------------------------------*/

/*! This method changes the angle of shoulder, if currently value is near
    the current angle of shoulder then no changes are made
    \param side which leg to change angle
    \param valueFront New angle ( for moving the should towards front and back )
    \param valueOpen New angle ( for Opening and closing the arms )
    \param dSpeed speed to set for action */
bool BasicAgent::shoulder( SideT side, AngDeg valueFront, AngDeg valueOpen, double dSpeed )
{
  bool bResult;
  if( dSpeed == -1 )
    dSpeed = static_gain;

  JointT joint = ( side == SIDE_LEFT ) ? JID_LARM_1_2 : JID_RARM_1_2;
  AngDeg curAngle = WM->getJoint( joint ).getAxis();
  AngDeg angFront, angOpen;

  // First Front/Back
  if( fabs( curAngle - valueFront ) > static_maxError )
  {
    angFront = dSpeed * (valueFront - curAngle);
    bResult = false;
  }
  else
  {
    angFront = 0;
    bResult = true;
  }

  // Then Open/Close
  curAngle = WM->getJoint( joint ).getAxis2();
  if( fabs( curAngle - valueOpen ) > static_maxError )
  {
    angOpen = dSpeed * (valueOpen - curAngle);
    bResult &= false;
  }
  else
  {
    angOpen = 0;
    bResult &= true;
  }

  adjustJoint( joint, angFront, angOpen );
  return bResult;
}

/*! This method changes the angle of hip, if currently value is near
    the current angle of hip then no changes are made
    \param side which leg to change angle
    \param valueFront New angle ( for moving the should towards front and back )
    \param valueOpen New angle ( for Opening and closing the arms )
    \param dSpeed speed to set for action */
bool BasicAgent::hip( SideT side, AngDeg valueFront, AngDeg valueOpen, double dSpeed )
{
  bool bResult;

  if( dSpeed == -1 )
    dSpeed = static_gain;

  JointT joint = ( side == SIDE_LEFT ) ? hipL : hipR;
  AngDeg curAngle = WM->getJoint( joint ).getAxis();
  AngDeg angFront, angOpen;

  // First Front/Back
  if( fabs( curAngle - valueFront ) > static_maxError )
  {
    angFront = dSpeed * (valueFront - curAngle);
    bResult = false;
  }
  else
  {
    angFront = 0;
    bResult = true;
  }

  // Then Open/Close
  curAngle = WM->getJoint( joint ).getAxis2();
  if( fabs( curAngle - valueOpen ) > static_maxError )
  {
    angOpen = dSpeed * (valueOpen - curAngle);
    bResult &= false;
  }
  else
  {
    angOpen = 0;
    bResult &= true;
  }

  adjustJoint( joint, angFront, angOpen );
  return bResult;
}

/*! This method changes the angle of shoulder, if currently value is near
    the current angle of hip then no changes are made
    \param side which leg to change angle
    \param valueFront new angle to change to 
    \param valueOpen new angle to change to
    \param dSpeed speed to set for action */
bool BasicAgent::ankle( SideT side, AngDeg valueFront, AngDeg valueOpen, double dSpeed )
{
  bool bResult;

  if( dSpeed == -1 )
    dSpeed = static_gain;

  JointT joint = ( side == SIDE_LEFT ) ? ankleL : ankleR;
  AngDeg curAngle = WM->getJoint( joint ).getAxis();
  AngDeg angFront, angOpen;

  // First Front/Back
  if( fabs( curAngle - valueFront ) > static_maxError )
  {
    angFront = dSpeed * (valueFront - curAngle);
    bResult = false;
  }
  else
  {
    angFront = 0;
    bResult = true;
  }

  // Then Open/Close
  curAngle = WM->getJoint( joint ).getAxis2();
  if( fabs( curAngle - valueOpen ) > static_maxError )
  {
    angOpen = dSpeed * (valueOpen - curAngle);
    bResult &= false;
  }
  else
  {
    angOpen = 0;
    bResult &= true;
  }