Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/BehaviorControl/GT2005BehaviorControl/GT2005BasicBehaviors/GT2005PotentialFieldBasicBehaviors.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005PotentialFieldBasicBehaviors.cpp
    00003 *
    00004 * Implementation of basic behaviors defined in potential-field-basic-behaviors.xml.
    00005 *
    00006 * @author <a href="mailto:timlaue@tzi.de">Tim Laue</a>
    00007 */
    00008
    00009 #include "GT2005PotentialFieldBasicBehaviors.h"
    00010 #include "Tools/FieldDimensions.h"
    00011
    00012
    00013
    00014 GT2005PotentialFieldBasicBehaviorSupport::GT2005PotentialFieldBasicBehaviorSupport(
    00015                                             const BehaviorControlInterfaces& interfaces,
    00016                                             Xabsl2ErrorHandler& errorHandler):
    00017             GT2005PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-support","GT2005/support.pfc")
    00018 {
    00019   registerParameter("potential-field-support.x", x);
    00020   registerParameter("potential-field-support.y", y);
    00021   registerParameter("potential-field-support.max-speed", maxSpeed);
    00022 }
    00023
    00024
    00025 void GT2005PotentialFieldBasicBehaviorSupport::execute()
    00026 {
    00027   Pose2D destination(0.0,x,y);
    00028   Pose2D dummy;
    00029   potentialfields.execute(robotPose, ballModel, playerPoseCollection,
    00030                           obstaclesModel, teamMessageCollection, destination, dummy, 1, result);
    00031   motionRequest.motionType = MotionRequest::walk;
    00032   motionRequest.walkRequest.walkType = WalkRequest::normal;
    00033
    00034   Vector2<double> relVec(-robotPose.translation + destination.translation);
    00035   double distanceToPoint(relVec.abs());
    00036   if(fabs(result.motion.speed) > 1.0) result.motion.speed = 1.0;
    00037   motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.speed * result.motion.pos.x;
    00038   motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.speed * result.motion.pos.y;
    00039   motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
    00040   if((distanceToPoint < 50.0) && fabs(toDegrees(result.motion.rotation)) < 15)
    00041   {
    00042     motionRequest.motionType = MotionRequest::stand;
    00043   }
    00044   else if(distanceToPoint < 50.0)
    00045   {
    00046     motionRequest.walkRequest.walkParams.translation.x = 0.0;
    00047     motionRequest.walkRequest.walkParams.translation.y = 0.0;
    00048   }
    00049   else if(distanceToPoint < 300.0)
    00050   {
    00051     motionRequest.walkRequest.walkParams.translation *= distanceToPoint/300.0;
    00052   }
    00053 }
    00054
    00055
    00056 GT2005PotentialFieldBasicBehaviorOffensiveSupport::GT2005PotentialFieldBasicBehaviorOffensiveSupport(
    00057                                             const BehaviorControlInterfaces& interfaces,
    00058                                             Xabsl2ErrorHandler& errorHandler):
    00059             GT2005PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-offensive-support","GT2005/offsupp.pfc")
    00060 {
    00061   registerParameter("potential-field-offensive-support.x", x);
    00062   registerParameter("potential-field-offensive-support.y", y);
    00063   registerParameter("potential-field-offensive-support.max-speed", maxSpeed);
    00064 }
    00065
    00066
    00067 void GT2005PotentialFieldBasicBehaviorOffensiveSupport::execute()
    00068 {
    00069   Pose2D destination(0.0,x,y);
    00070   //Compute a position to rotate to:
    00071   Pose2D destination2;
    00072   Vector2<double> knownBallPos(ballModel.getKnownPosition(BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted));
    00073   Vector2<double> knownBallPosRobotRelative(Geometry::vectorTo(robotPose, knownBallPos));
    00074   Vector2<double> rP(robotPose.translation);
    00075   //double relAngleToBall(Vector2<double>(knownBallPos-rP).angle());
    00076   Vector2<double> absGoalVec((double)xPosOpponentGoal,0.0);
    00077   //double relAngleToGoal((absGoalVec-rP).angle());
    00078  /* if(fabs( relAngleToBall - relAngleToGoal) < pi/2)
    00079   {
    00080     double desAngle(((relAngleToBall + relAngleToGoal) /2) - (fabs(relAngleToBall) < fabs(relAngleToGoal) ? relAngleToBall:relAngleToGoal));
    00081     destination2.translation = Geometry::vectorTo(robotPose, Vector2<double>(rP.x+cos(desAngle),rP.y+sin(desAngle)));
    00082   }
    00083
    00084   else
    00085   {
    00086      destination2.translation = knownBallPosRobotRelative;
    00087   }*/
    00088   destination2.translation = knownBallPos;
    00089   //Execute potential field behavior:
    00090   potentialfields.execute(robotPose, ballModel, playerPoseCollection,
    00091                           obstaclesModel, teamMessageCollection, destination, destination2, 2, result);
    00092   //Set motion request:
    00093   motionRequest.motionType = MotionRequest::walk;
    00094   motionRequest.walkRequest.walkType = WalkRequest::normal;
    00095
    00096   Vector2<double> relVec(-robotPose.translation + destination.translation);
    00097   double distanceToPoint(relVec.abs());
    00098   if(fabs(result.motion.speed) > 1.0) result.motion.speed = 1.0;
    00099   motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.speed * result.motion.pos.x;
    00100   motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.speed * result.motion.pos.y;
    00101   motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
    00102   if((distanceToPoint < 50.0) && fabs(toDegrees(result.motion.rotation)) < 15)
    00103   {
    00104     motionRequest.motionType = MotionRequest::stand;
    00105   }
    00106   else if(distanceToPoint < 50.0)
    00107   {
    00108     motionRequest.walkRequest.walkParams.translation.x = 0.0;
    00109     motionRequest.walkRequest.walkParams.translation.y = 0.0;
    00110   }
    00111   else if(distanceToPoint < 300.0)
    00112   {
    00113     motionRequest.walkRequest.walkParams.translation *= distanceToPoint/300.0;
    00114   }
    00115 }
    00116
    00117
    00118 GT2005PotentialFieldBasicBehaviorAvoidRobots::GT2005PotentialFieldBasicBehaviorAvoidRobots(
    00119                                               const BehaviorControlInterfaces& interfaces,
    00120                                               Xabsl2ErrorHandler& errorHandler):
    00121             GT2005PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-avoid-robots","GT2005/avoidbot.pfc")
    00122 {
    00123   registerParameter("potential-field-avoid-robots.x", x);
    00124   registerParameter("potential-field-avoid-robots.y", y);
    00125   registerParameter("potential-field-avoid-robots.max-speed", maxSpeed);
    00126 }
    00127
    00128
    00129 void GT2005PotentialFieldBasicBehaviorAvoidRobots::execute()
    00130 {
    00131   Pose2D destination(0.0,x,y);
    00132   Pose2D dummy;
    00133   potentialfields.execute(robotPose, ballModel, playerPoseCollection,
    00134                           obstaclesModel, teamMessageCollection, destination, dummy, 1, result);
    00135   motionRequest.motionType = MotionRequest::walk;
    00136   motionRequest.walkRequest.walkType = WalkRequest::normal;
    00137
    00138   Vector2<double> relVec(-robotPose.translation + destination.translation);
    00139   double distanceToPoint(relVec.abs());
    00140   if(fabs(result.motion.speed) > 1.0) result.motion.speed = 1.0;
    00141   motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.speed * result.motion.pos.x;
    00142   motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.speed * result.motion.pos.y;
    00143   motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
    00144   if((distanceToPoint < 50.0) && fabs(toDegrees(result.motion.rotation)) < 15)
    00145   {
    00146     motionRequest.motionType = MotionRequest::stand;
    00147   }
    00148   else if(distanceToPoint < 50.0)
    00149   {
    00150     motionRequest.walkRequest.walkParams.translation.x = 0.0;
    00151     motionRequest.walkRequest.walkParams.translation.y = 0.0;
    00152   }
    00153   else if(distanceToPoint < 300.0)
    00154   {
    00155     motionRequest.walkRequest.walkParams.translation *= distanceToPoint/300.0;
    00156   }
    00157 }
    00158