From 189b02f4b61ccb6ee8d591ce85ecc806c085e6ae Mon Sep 17 00:00:00 2001 From: Francesco Riccio Date: Mon, 2 Dec 2013 10:59:22 +0100 Subject: [PATCH] attacker fix && rewards print --- .../{InvAttackerPNP.h => AttackerPNP.h} | 58 ++++++++++++------- machineLearning/DiveHandler/DiveHandler.cpp | 20 ++++--- machineLearning/GoalieLearner.h | 4 +- 3 files changed, 50 insertions(+), 32 deletions(-) rename machineLearning/{InvAttackerPNP.h => AttackerPNP.h} (67%) diff --git a/machineLearning/InvAttackerPNP.h b/machineLearning/AttackerPNP.h similarity index 67% rename from machineLearning/InvAttackerPNP.h rename to machineLearning/AttackerPNP.h index dc30751..3933461 100644 --- a/machineLearning/InvAttackerPNP.h +++ b/machineLearning/AttackerPNP.h @@ -3,9 +3,9 @@ #include // Uncomment if you want to have debug information. -//#define DEBUG_BEHAVIOR +#define DEBUG_BEHAVIOR -#define MIN_DIST_TO_BALL 150 +#define MIN_DIST_TO_BALL 200 using namespace PTracking; using namespace SPQR; @@ -16,7 +16,7 @@ option AttackerPNP bool isLeftFoot() { - if(theBallModel.estimate.position.y > -0.5) return true; + if(theBallModel.estimate.position.y > 0) return true; else return false; } @@ -33,7 +33,7 @@ option AttackerPNP bool alignedTowardGoal() { Vector2 goal_position = (theFreePartOfOpponentGoalModel.leftEnd + theFreePartOfOpponentGoalModel.rightEnd) / 2; - double goal_angle = atan2(-goal_position.y, -goal_position.x); + double goal_angle = atan2(goal_position.y,goal_position.x); if (fabs( goal_angle ) < deg2rad(3)) return true; else return false; @@ -80,7 +80,7 @@ option AttackerPNP theMotionRequest.walkRequest.mode = WalkRequest::targetMode; theMotionRequest.walkRequest.target.translation.x = 0; theMotionRequest.walkRequest.target.translation.y = 0; - theMotionRequest.walkRequest.target.rotation = theBallModel.estimate.position.y < 0 ? -M_PI : M_PI; + theMotionRequest.walkRequest.target.rotation = theBallModel.estimate.velocity.y < 0 ? -M_PI : M_PI; theMotionRequest.walkRequest.speed.translation.x = 0; theMotionRequest.walkRequest.speed.translation.y = 0; theMotionRequest.walkRequest.speed.rotation = 1; @@ -130,7 +130,7 @@ option AttackerPNP action { position = (theFreePartOfOpponentGoalModel.leftEnd + theFreePartOfOpponentGoalModel.rightEnd) / 2; - angle = atan2(-position.y, -position.x); + angle = atan2(position.y,position.x); theMotionRequest.walkRequest.mode = WalkRequest::targetMode; theMotionRequest.walkRequest.target.translation.x = theBallModel.estimate.position.x - 150; @@ -153,8 +153,10 @@ option AttackerPNP stateName = "Kick"; #endif - if (theFrameInfo.time - theBallModel.timeWhenLastSeen > 2000) return FindBall; - if (stateTime > 2000) return ApproachBall; + if (theFrameInfo.time - theBallModel.timeWhenLastSeen > 1000) return FindBall; + + if (stateTime > 2000) return ApproachBall; + else if( norm(theBallModel.estimate.position.x, theBallModel.estimate.position.y) < MIN_DIST_TO_BALL ) return LookYourFeet; } action { @@ -171,23 +173,36 @@ option AttackerPNP stateName = "ApproachBall"; #endif - if (theFrameInfo.time - theBallModel.timeWhenLastSeen > 2000) return FindBall; + if (theFrameInfo.time - theBallModel.timeWhenLastSeen > 1000) return FindBall; if( norm(theBallModel.estimate.position.x, theBallModel.estimate.position.y) < MIN_DIST_TO_BALL ) return LookYourFeet; } action { - theMotionRequest.walkRequest.mode = WalkRequest::targetMode; - theMotionRequest.walkRequest.target.translation.x = theBallModel.estimate.position.x; - theMotionRequest.walkRequest.target.translation.y = theBallModel.estimate.position.y; - theMotionRequest.walkRequest.target.rotation = atan2(theBallModel.estimate.position.y,theBallModel.estimate.position.x); - theMotionRequest.walkRequest.speed.translation.x = SPQR::SPEED_X; - theMotionRequest.walkRequest.speed.translation.y = SPQR::SPEED_Y; - theMotionRequest.walkRequest.speed.rotation = 1; - theMotionRequest.motion = MotionRequest::walk; - - LookAtBall(); + theMotionRequest.walkRequest.mode = WalkRequest::targetMode; + theMotionRequest.walkRequest.target.translation.x = theBallModel.estimate.position.x; + + if( isLeftFoot() ) + { + theMotionRequest.walkRequest.target.translation.y = theBallModel.estimate.position.y - 50.0; + theMotionRequest.walkRequest.target.rotation = atan2(theBallModel.estimate.position.y - 50.0,theBallModel.estimate.position.x); + } + else + { + theMotionRequest.walkRequest.target.translation.y = theBallModel.estimate.position.y + 50.0; + theMotionRequest.walkRequest.target.rotation = atan2(theBallModel.estimate.position.y + 50.0,theBallModel.estimate.position.x); + } + +// theMotionRequest.walkRequest.target.translation.y = theBallModel.estimate.position.y; + + theMotionRequest.walkRequest.target.rotation = atan2(theBallModel.estimate.position.y,theBallModel.estimate.position.x); + theMotionRequest.walkRequest.speed.translation.x = SPQR::SPEED_X; + theMotionRequest.walkRequest.speed.translation.y = SPQR::SPEED_Y; + theMotionRequest.walkRequest.speed.rotation = 1; + theMotionRequest.motion = MotionRequest::walk; + + LookAtBall(); } } @@ -198,7 +213,7 @@ option AttackerPNP #ifdef DEBUG_BEHAVIOR stateName = "MoveAroundBall"; #endif - if (theFrameInfo.time - theBallModel.timeWhenLastSeen > 2000) return FindBall; + if (theFrameInfo.time - theBallModel.timeWhenLastSeen > 2000) return FindBall; if( norm(theBallModel.estimate.position.x, theBallModel.estimate.position.y) > MIN_DIST_TO_BALL ) return ApproachBall; else @@ -207,13 +222,12 @@ option AttackerPNP else if(fabs(angle) < Utils::deg2rad(TURN_VALID_THS) && fabs(theBallModel.estimate.position.y) < 100 && fabs(theBallModel.estimate.position.x) < 350) return AlignTowardGoal; - else return Kick; } } action { position = (theFreePartOfOpponentGoalModel.leftEnd + theFreePartOfOpponentGoalModel.rightEnd) / 2; - angle = atan2(-position.y, -position.x); + angle = atan2(position.y,position.x); theMotionRequest.walkRequest.mode = WalkRequest::targetMode; theMotionRequest.walkRequest.target.translation.x = theBallModel.estimate.position.x - MIN_DIST_TO_BALL; diff --git a/machineLearning/DiveHandler/DiveHandler.cpp b/machineLearning/DiveHandler/DiveHandler.cpp index 396d5c6..6163a0b 100644 --- a/machineLearning/DiveHandler/DiveHandler.cpp +++ b/machineLearning/DiveHandler/DiveHandler.cpp @@ -21,8 +21,8 @@ #include "DiveHandler.h" // Uncomment to have debug information -#define DIVEHANDLER_DEBUG -#define DIVEHANDLER_TRAINING +//#define DIVEHANDLER_DEBUG +//#define DIVEHANDLER_TRAINING //#define RAND_PERMUTATIONS #define NEGATIVE_REWARD -1.0 @@ -541,7 +541,7 @@ void DiveHandler::update(DiveHandle& diveHandle) if (theRobotInfo.number == 1) { // Check if the goalie is currently performing a dive - if( (diveHandle.diveTime >= 0.0) && (diveHandle.diveTime < SPQR::GOALIE_DIVE_TIME_TOLERANCE) ) + if( /*(diveHandle.diveTime >= 0.0) &&*/ (diveHandle.diveTime < SPQR::GOALIE_DIVE_TIME_TOLERANCE) ) dived = true; // Compute the ball projection estimate @@ -573,6 +573,7 @@ void DiveHandler::update(DiveHandle& diveHandle) state = waitReward; // Flag a pending reward to the goalie behavior diveHandle.rewardAck = false; + SPQR_ERR("waiting reward"); } else // The algorithm has converged: turning off learning @@ -600,11 +601,12 @@ void DiveHandler::update(DiveHandle& diveHandle) // A reward has been received: re-enable learning state = learning; // Clear the pending reward - if (!diveHandle.rewardAck) + if(!diveHandle.rewardAck) diveHandle.rewardAck = true; + SPQR_SUCCESS("reward accepted"); // Since the opponent team scored, the dive action should be done - if (dived) dived = false; + if(dived) dived = false; } // The goalie has performed a dive and yet the outcome is unknown else if(dived && (theFrameInfo.time - theBallModel.timeWhenLastSeen) < 500) @@ -624,8 +626,9 @@ void DiveHandler::update(DiveHandle& diveHandle) // A reward has been received: re-enable learning state = learning; // Clear the pending reward - if (!diveHandle.rewardAck) + if(!diveHandle.rewardAck) diveHandle.rewardAck = true; + SPQR_SUCCESS("reward accepted"); // Since the outcome is known, the dive action is done dived = false; @@ -650,8 +653,9 @@ void DiveHandler::update(DiveHandle& diveHandle) // A reward has been received: re-enable learning state = learning; // Clear the pending reward - if (!diveHandle.rewardAck) + if(!diveHandle.rewardAck) diveHandle.rewardAck = true; + SPQR_SUCCESS("reward accepted"); // Since the outcome is known, the dive action is done dived = false; @@ -681,7 +685,7 @@ void DiveHandler::update(DiveHandle& diveHandle) #ifdef DIVEHANDLER_TRAINING if (diveTime > 0.0) { - if (diveHandle.diveTime < SPQR::GOALIE_DIVE_TIME_TOLERANCE) + if(diveHandle.diveTime < SPQR::GOALIE_DIVE_TIME_TOLERANCE) SPQR_INFO("Dive now! "); } #endif diff --git a/machineLearning/GoalieLearner.h b/machineLearning/GoalieLearner.h index 2d0cf9f..2d3a068 100644 --- a/machineLearning/GoalieLearner.h +++ b/machineLearning/GoalieLearner.h @@ -4,7 +4,7 @@ // Uncomment if you want to have debug information #define GOALIE_DEBUG_MODE -option GoalieLearningBehavior +option GoalieLearner { private: @@ -297,7 +297,7 @@ option GoalieLearningBehavior { decision { - if(theDiveHandle.rewardAck) return main_loop; + if(theDiveHandle.rewardAck && ballIsSeen()) return main_loop; } action {