basicagent.cpp

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

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

/*--------------------------------------------------------------------------*/
/*--------------------------  Turning Procedures  --------------------------*/
/*--------------------------------------------------------------------------*/


/*! This procedure turns the agent to a desired angle by selecting
    the way agent should turn; ie. with his left leg or right one.
    \param ang Desired angle to turn the agent to
    \param start static value to synchronize moves
*/
bool BasicAgent::turnTo( AngDeg ang, bool &start )
{
  ang = VecPosition::normalizeAngle( ang );
  ang = max( ang, -90 );
  ang = min( ang,  90 );

  if( ang > 0 )
    return turnToLeft( ang, start );
  else
    return turnToRight( ang, start );
}

/*! This methods turn the agent to gain the angle requested,
    First Agent moves its weight on his first foot, then rotates, then
    puts its weigth on the other foot and at last finishes turning.
    \param ang Angle to turn to
    \param start sets weather this should be the first call of this method
    \return bool true indicates that turning is finished
*/
bool BasicAgent::turnToLeft( AngDeg ang, bool & start )
{
  double gain = 0.02;

  static Time time;
  if( start )
  {
    start = false;
    time  = WM->getSimulatorTime();
  }

  if( WM->getSimulatorTime() - time < 2 )
  {
    return false;
  }
  else if( ( WM->getSimulatorTime() - time ) < 3 )
  {
    // In this step which takes 1 second, agent turns his right ankle ( 25 degs & 5 gains ),
    // & bending his left hip ( 12 degrees & 1 gain ) so that she (the agent :) )
    // keeps her weight in balance

    // Left Part Configuration                       // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.50 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,   10, 1.50 * gain );    hip     ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    ankle   ( SIDE_LEFT,    0,    0, 1.00 * gain );    ankle   ( SIDE_RIGHT,    0,  -25, 5.00 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( ( WM->getSimulatorTime() - time ) < 7 )
  {
    // In this step agent stops pushing from the opposite foot also he exerts a little
    // force by the ankle of following leg ankle ( 5 degs and 1 gain ) to stop free
    // falling into the unknown! ; also mentioning the role of left hip in
    // controling the balance ( 15 degs & 1 gain )
    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,   12, 1.50 * gain );    hip     ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    ankle   ( SIDE_LEFT,    0,    5, 1.00 * gain );    ankle   ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( WM->getSimulatorTime() - time < 8.5 )
  {
    // In this step agent stabilizes its weight on foot by changing the left hip angle
    // ( 15 degs & 1 gain ), also changing his left ankle angle by ( -7 degs & 1 gain )
    // and of course changing right hip angle by ( -10 degs & -10 degs & 1 gain )

    // At last he changes his own bodies angle by rotating a leg which is attached to ground

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,   15, 1.00 * gain );    hip     ( SIDE_RIGHT,  -10,  -10, 1.00 * gain );
    ankle   ( SIDE_LEFT,    0,  -10, 2.00 * gain );    ankle   ( SIDE_RIGHT,    8,    0, 1.00 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );

    rotateLeg( SIDE_LEFT, -ang, gain );
    return false;
  }
  else if( WM->getSimulatorTime() - time < 12)
  {
    // In this last step agent stabilizes its weight on both feet by re-initializing
    // almost all joints to 0 degrees

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 2.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 2.00 * gain );
    hip     ( SIDE_LEFT,    0,    0, 2.00 * gain );    hip     ( SIDE_RIGHT,    0,    0, 2.00 * gain );
    ankle   ( SIDE_LEFT,    0,    0, 2.00 * gain );    ankle   ( SIDE_RIGHT,    0,    0, 2.00 * gain );
    knee    ( SIDE_LEFT,    0,       2.00 * gain );    knee    ( SIDE_RIGHT,    0,       2.00 * gain );
    elbow   ( SIDE_LEFT,    0,       2.00 * gain );    elbow   ( SIDE_RIGHT,    0,       2.00 * gain );

    rotateLeg( SIDE_LEFT, 0, 3 * gain );
    return false;
  }

  return true;
}

/*! This methods turn the agent to gain the angle requested,
    First Agent moves its weight on his first foot, then rotates, then
    puts its weigth on the other foot and at last finishes turning.
    \param ang Angle to turn to
    \param start sets weather this should be the first call of this method
    \return bool true indicates that turning is finished
*/
bool BasicAgent::turnToRight( AngDeg ang, bool & start )
{
  double gain = 0.02;

  static Time time;
  if( start )
  {
    start = false;
    time  = WM->getSimulatorTime();
  }

  if( WM->getSimulatorTime() - time < 2 )
  {
    return false;
  }
  else if( ( WM->getSimulatorTime() - time ) < 3 )
  {
    // In this step which takes 1 second, agent turns his right ankle ( 25 degs & 5 gains ),
    // & bending his left hip ( 12 degrees & 1 gain ) so that she (the agent :) )
    // keeps her weight in balance

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.50 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,    0, 1.50 * gain );    hip     ( SIDE_RIGHT,    0,  -10, 1.50 * gain );
    ankle   ( SIDE_LEFT,    0,   25, 5.00 * gain );    ankle   ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( ( WM->getSimulatorTime() - time ) < 7 )
  {
    // In this step agent stops pushing from the opposite foot also he exerts a little
    // force by the ankle of following leg ankle ( 5 degs and 1 gain ) to stop free
    // falling into the unknown! ; also mentioning the role of left hip in
    // controling the balance ( 15 degs & 1 gain )
    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,    0, 1.00 * gain );    hip     ( SIDE_RIGHT,    0,  -12, 1.50 * gain );
    ankle   ( SIDE_LEFT,    0,    0, 1.00 * gain );    ankle   ( SIDE_RIGHT,    0,   -5, 1.00 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( WM->getSimulatorTime() - time < 8.5 )
  {
    // In this step agent stabilizes its weight on foot by changing the left hip angle
    // ( 15 degs & 1 gain ), also changing his left ankle angle by ( -7 degs & 1 gain )
    // and of course changing right hip angle by ( -10 degs & -10 degs & 1 gain )

    // At last he changes his own bodies angle by rotating a leg which is attached to ground

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,  -10,   10, 1.00 * gain );    hip     ( SIDE_RIGHT,    0,  -15, 1.00 * gain );
    ankle   ( SIDE_LEFT,    8,    0, 1.00 * gain );    ankle   ( SIDE_RIGHT,    0,   10, 2.00 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );

    rotateLeg( SIDE_RIGHT, -ang, gain );
    return false;
  }
  else if( WM->getSimulatorTime() - time < 12)
  {
    // In this last step agent stabilizes its weight on both feet by re-initializing
    // almost all joints to 0 degrees

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,    0, 1.00 * gain );    hip     ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    ankle   ( SIDE_LEFT,    0,    0, 1.00 * gain );    ankle   ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,      1.00 * gain );

    rotateLeg( SIDE_RIGHT, 0, 3 * gain );
    return false;
  }

  return true;
}

/*--------------------------------------------------------------------------*/
/*-------------------------- Walking Procedures ----------------------------*/
/*--------------------------------------------------------------------------*/

/*! This method is used to walk the agent
    \param start boolean vlaue which synchronizes agents movements
    \return bool indicating wheather moving is finished
*/
bool BasicAgent::walk( bool & start )
{
  double gain = 0.02;

  static Time time;
  if( start )
  {
    start = false;
    time  = WM->getSimulatorTime();
  }

  double deltaTime = WM->getSimulatorTime() - time;

  if( deltaTime < 2 )
  {
    return false;
  }
  else if( deltaTime < 2.5 )
  {
    ///                                   Step 1
    /// --> Miare roye paye rast badesh, paharam baraye avalin bare rotate mikone
    /// --> HADAF: bordane ghesmati az taodol be samte rast baraye baz shodan
    ///     ja baraye bordan paye chap be jelo

    // Left Part Configuration                       // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,    0, 1.00 * gain );    hip     ( SIDE_RIGHT,    0,   -5, 1.00 * gain );
    ankle   ( SIDE_LEFT,    0,   10, 10.0 * gain );    ankle   ( SIDE_RIGHT,    0,   -5, 1.00 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );

    rotateLeg( SIDE_LEFT, 10, gain );
    rotateLeg( SIDE_RIGHT, -10, gain );
    return false;
  }
  else if( deltaTime < 3.2 )
  {
    ///                                   Step 2
    /// --> Paye chap ro miare jelo, paye rasto yezare kaj mikone ta agent bioofte jeloii
    /// --> HADAF: Bordane paye chap be jelo

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,   25,    0, 2.50 * gain );    hip     ( SIDE_RIGHT,    0,   -5, 1.00 * gain );
    ankle   ( SIDE_LEFT,   10,    0, 2.50 * gain );    ankle   ( SIDE_RIGHT,    5,   -5, 1.00 * gain );
    knee    ( SIDE_LEFT,  -25,       2.50 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( deltaTime < 4.5 )
  {
    ///                                   Step 3
    /// --> Paye rasto saf mikonim ta agent badanesh saf beshe
    /// --> HADAF: gozashtan ghadame chap be zamin, rast kardan ghamate agent

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,   25,    0, 1.50 * gain );    hip     ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    ankle   ( SIDE_LEFT,   10,    0, 1.50 * gain );    ankle   ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    knee    ( SIDE_LEFT,  -25,       1.50 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( deltaTime < 5 )
  {
    ///                                   Step 4
    /// --> Paye chap ro saf mikone ta ye niro be jelo bede va be andazeye kafi ertefa be
    ///     agent bede ta agent betoone paye rastesho harekat bede, dar zemn paye raste
    ///     agent ro az ghasd kham mikonim ta hengame jelo avordan too step ba'd be zamin nakhore
    /// --> HADAF: saf kardane badane agent

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,    5, 1.00 * gain );    hip     ( SIDE_RIGHT,    0,    0, 1.50 * gain );
    ankle   ( SIDE_LEFT,    0,    5, 2.00 * gain );    ankle   ( SIDE_RIGHT,   10,    0, 2.50 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,  -10,       2.50 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( deltaTime < 6.2 )
  {
    ///                                   Step 5
    /// --> Paye chap ro saf karde hala paye rasto az aghab barmidare mizare jelo,
    ///     tarjihan :-) joori ke be zamin nasabe!
    /// --> HADAF: gozashtane paye rast be jelo, saf kardane badane agent

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,    0, 1.00 * gain );    hip     ( SIDE_RIGHT,   25,    0, 1.50 * gain );
    ankle   ( SIDE_LEFT,    0,    0, 1.00 * gain );    ankle   ( SIDE_RIGHT,   10,    0, 1.50 * gain );
    knee    ( SIDE_LEFT,    0,       1.00 * gain );    knee    ( SIDE_RIGHT,  -25,       1.50 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( deltaTime < 6.7 )
  {
    ///                                   Step 6
    /// --> Paye rast ro saf mikone ta ye niro be jelo bede va be andazeye kafi ertefa be
    ///     agent bede ta agent betoone paye chapesho harekat bede, dar zemn paye chape
    ///     agent ro az ghasd kham mikonim ta hengame jelo avordan too step ba'd be zamin nakhore
    /// --> HADAF: saf kardane badane agent

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,    0,    0, 1.50 * gain );    hip     ( SIDE_RIGHT,    0,   -5, 1.00 * gain );
    ankle   ( SIDE_LEFT,   10,    0, 2.50 * gain );    ankle   ( SIDE_RIGHT,    0,   -5, 2.00 * gain );
    knee    ( SIDE_LEFT,  -10,       2.50 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else if( deltaTime < 7.9 )
  {
    ///                                   Step 7
    /// --> Paye rast ro saf karde hala paye chapo az aghab barmidare mizare jelo,
    ///     tarjihan :-) joori ke be zamin nasabe!
    /// --> HADAF: gozashtane paye chap be jelo, saf kardane badane agent

    // Left Part Configuration                                  // Right Part Configuration
    shoulder( SIDE_LEFT,    0,    0, 1.00 * gain );    shoulder( SIDE_RIGHT,    0,    0, 1.00 * gain );
    hip     ( SIDE_LEFT,   25,    0, 1.50 * gain );    hip     ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    ankle   ( SIDE_LEFT,   10,    0, 1.50 * gain );    ankle   ( SIDE_RIGHT,    0,    0, 1.00 * gain );
    knee    ( SIDE_LEFT,  -25,       1.50 * gain );    knee    ( SIDE_RIGHT,    0,       1.00 * gain );
    elbow   ( SIDE_LEFT,    0,       1.00 * gain );    elbow   ( SIDE_RIGHT,    0,       1.00 * gain );
    return false;
  }
  else
  {
    time = WM->getSimulatorTime() - 4.5;
  }

  return true;
}