Modules/BehaviorControl/GT2004BehaviorControl/GT2004BasicBehaviors/GT2004PotentialFieldBasicBehaviors.cpp

Go to the documentation of this file.

/** 
* @file GT2004PotentialFieldBasicBehaviors.cpp
*
* Implementation of basic behaviors defined in potential-field-basic-behaviors.xml.
*
* @author <a href="mailto:timlaue@tzi.de">Tim Laue</a>
*/

#include "GT2004PotentialFieldBasicBehaviors.h"
#include "Tools/FieldDimensions.h"


GT2004PotentialFieldBasicBehaviorGoToPose::GT2004PotentialFieldBasicBehaviorGoToPose(const BehaviorControlInterfaces& interfaces,
                                            Xabsl2ErrorHandler& errorHandler):
            GT2004PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-go-to-pose","GT2004/gotopose.pfc")
{
  registerParameter("potential-field-go-to-pose.x", x);
  registerParameter("potential-field-go-to-pose.y", y);
  registerParameter("potential-field-go-to-pose.destination-angle", destinationAngle);
  registerParameter("potential-field-go-to-pose.max-speed", maxSpeed);
}


void GT2004PotentialFieldBasicBehaviorGoToPose::execute()
{
  Pose2D destination1(fromDegrees(destinationAngle),x,y);
  Pose2D dummyDestination;
  potentialfields.execute(robotPose, ballModel, playerPoseCollection, 
                          obstaclesModel, teamMessageCollection, destination1, dummyDestination, 1, result);

  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  Vector2<double> relVec(-robotPose.translation + destination1.translation);
  double distanceToPoint(relVec.abs());
  if(result.action == "do-turn")
  {
    double rotationDifference = destination1.rotation - robotPose.rotation;
    while(rotationDifference>pi) rotationDifference -= 2*pi;
    while(rotationDifference<-pi) rotationDifference += 2*pi;
    if(fabs(toDegrees(rotationDifference))<10)
    {
      if(distanceToPoint < 50.0)
      {
        motionRequest.motionType = MotionRequest::stand;
      }
      else
      {
        double ca(cos(-robotPose.rotation));
        double sa(sin(-robotPose.rotation));
        double newX(relVec.x*ca - relVec.y*sa);
        relVec.y = relVec.y*ca + relVec.x*sa;
        relVec.x = newX;
        motionRequest.walkRequest.walkParams.translation = relVec;
        motionRequest.walkRequest.walkParams.translation.normalize();
        motionRequest.walkRequest.walkParams.translation *= 60.0;
        motionRequest.walkRequest.walkParams.rotation = 0;
      }
    }
    else
    {
      motionRequest.walkRequest.walkParams.translation.x = 0;
      motionRequest.walkRequest.walkParams.translation.y = 0;
      motionRequest.walkRequest.walkParams.rotation = rotationDifference;
      if(fabs(toDegrees(rotationDifference)) < 30.0)
      {
        motionRequest.walkRequest.walkParams.rotation /= fabs(rotationDifference);
        motionRequest.walkRequest.walkParams.rotation *= fromDegrees(30.0);
      }
    }
  }
  else
  {
    motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.pos.x;
    motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.pos.y;
    motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
    if(distanceToPoint < 150)
    {
      motionRequest.walkRequest.walkParams.translation.x /= 3.0;
      motionRequest.walkRequest.walkParams.translation.y /= 3.0; 
    }
    if(fabs(toDegrees(motionRequest.walkRequest.walkParams.rotation)) > 60)
    {
      motionRequest.walkRequest.walkParams.translation.x = 0;
      motionRequest.walkRequest.walkParams.translation.y = 0; 
    }
  }
}


GT2004PotentialFieldBasicBehaviorSupport::GT2004PotentialFieldBasicBehaviorSupport(
                                            const BehaviorControlInterfaces& interfaces,
                                            Xabsl2ErrorHandler& errorHandler):
            GT2004PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-support","GT2004/support.pfc")
{
  registerParameter("potential-field-support.x", x);
  registerParameter("potential-field-support.y", y);
  registerParameter("potential-field-support.max-speed", maxSpeed);
}


void GT2004PotentialFieldBasicBehaviorSupport::execute()
{
  Pose2D destination(0.0,x,y);
  Pose2D dummy;
  potentialfields.execute(robotPose, ballModel, playerPoseCollection, 
                          obstaclesModel, teamMessageCollection, destination, dummy, 1, result);
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  
  Vector2<double> relVec(-robotPose.translation + destination.translation);
  double distanceToPoint(relVec.abs());
  if(fabs(result.motion.speed) > 1.0) result.motion.speed = 1.0;
  motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.speed * result.motion.pos.x;
  motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.speed * result.motion.pos.y;
  motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
  if((distanceToPoint < 50.0) && fabs(toDegrees(result.motion.rotation)) < 15)
  {
    motionRequest.motionType = MotionRequest::stand;
  }
  else if(distanceToPoint < 50.0)
  {
    motionRequest.walkRequest.walkParams.translation.x = 0.0;
    motionRequest.walkRequest.walkParams.translation.y = 0.0; 
  }
  else if(distanceToPoint < 150)
  {
    motionRequest.walkRequest.walkParams.translation.x /= 3.0;
    motionRequest.walkRequest.walkParams.translation.y /= 3.0; 
  }
}


GT2004PotentialFieldBasicBehaviorOffensiveSupport::GT2004PotentialFieldBasicBehaviorOffensiveSupport(
                                            const BehaviorControlInterfaces& interfaces,
                                            Xabsl2ErrorHandler& errorHandler):
            GT2004PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-offensive-support","GT2004/offsupp.pfc")
{
  registerParameter("potential-field-offensive-support.x", x);
  registerParameter("potential-field-offensive-support.y", y);
  registerParameter("potential-field-offensive-support.max-speed", maxSpeed);
}


void GT2004PotentialFieldBasicBehaviorOffensiveSupport::execute()
{
  Pose2D destination(0.0,x,y);
  //Compute a position to rotate to:
  Pose2D destination2;
  Vector2<double> knownBallPos(ballModel.getKnownPosition(BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted));
  Vector2<double> knownBallPosRobotRelative(Geometry::vectorTo(robotPose, knownBallPos));
  Vector2<double> rP(robotPose.translation);
  //double relAngleToBall(Vector2<double>(knownBallPos-rP).angle());
  Vector2<double> absGoalVec((double)xPosOpponentGoal,0.0);
  //double relAngleToGoal((absGoalVec-rP).angle());
 /* if(fabs( relAngleToBall - relAngleToGoal) < pi/2)
  {
    double desAngle(((relAngleToBall + relAngleToGoal) /2) - (fabs(relAngleToBall) < fabs(relAngleToGoal) ? relAngleToBall:relAngleToGoal));
    destination2.translation = Geometry::vectorTo(robotPose, Vector2<double>(rP.x+cos(desAngle),rP.y+sin(desAngle)));
  }

  else
  {
     destination2.translation = knownBallPosRobotRelative;
  }*/
  destination2.translation = knownBallPos;
  //Execute potential field behavior:
  potentialfields.execute(robotPose, ballModel, playerPoseCollection, 
                          obstaclesModel, teamMessageCollection, destination, destination2, 2, result);
  //Set motion request:
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  
  Vector2<double> relVec(-robotPose.translation + destination.translation);
  double distanceToPoint(relVec.abs());
  if(fabs(result.motion.speed) > 1.0) result.motion.speed = 1.0;
  motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.speed * result.motion.pos.x;
  motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.speed * result.motion.pos.y;
  motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
  if((distanceToPoint < 50.0) && fabs(toDegrees(result.motion.rotation)) < 15)
  {
    motionRequest.motionType = MotionRequest::stand;
  }
  else if(distanceToPoint < 50.0)
  {
    motionRequest.walkRequest.walkParams.translation.x = 0.0;
    motionRequest.walkRequest.walkParams.translation.y = 0.0; 
  }
  else if(distanceToPoint < 150)
  {
    motionRequest.walkRequest.walkParams.translation.x /= 3.0;
    motionRequest.walkRequest.walkParams.translation.y /= 3.0; 
  }
}


void GT2004PotentialFieldBasicBehaviors::registerBasicBehaviors(Xabsl2Engine& engine)
{
  engine.registerBasicBehavior(potentialFieldGoToPose);
  engine.registerBasicBehavior(potentialFieldSupport);
  engine.registerBasicBehavior(potentialFieldOffensiveSupport);
}


void GT2004PotentialFieldBasicBehaviors::update()
{
}

/* 
* $Log: GT2004PotentialFieldBasicBehaviors.cpp,v $
* Revision 1.7  2004/09/08 14:39:02  wachter
* - Fixed some doxygen-errors
*
* Revision 1.6  2004/07/01 12:27:45  dassler
* removed warning
*
* Revision 1.5  2004/06/18 11:18:09  tim
* temporary workaound
*
* Revision 1.4  2004/06/17 16:18:38  tim
* added potential field support behaviors
*
* Revision 1.3  2004/06/16 15:09:58  tim
* added potential field support behavior
*
* Revision 1.2  2004/06/02 17:18:23  spranger
* MotionRequest cleanup
*
* Revision 1.1  2004/05/25 12:59:39  tim
* added potential-field-go-to-pose
*
*/

---