From ff57c1f26202711b1631f8db493bc42050e86a1f Mon Sep 17 00:00:00 2001 From: jacksherling Date: Sun, 3 Mar 2024 21:38:19 -0500 Subject: [PATCH 1/4] fix oscillation and added block shot heuristic --- .../soccer/strategy/agent/position/seeker.cpp | 32 +++++++++++++------ .../soccer/strategy/agent/position/seeker.hpp | 3 +- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp index 4ea32af0959..c5aa6f2b754 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -37,7 +37,7 @@ rj_geometry::Point Seeker::calculate_open_point(double current_prec, double min_ while (current_prec > min_prec) { rj_geometry::Point ball_pos = world_state->ball.position; rj_geometry::Point min = current_point; - double min_val = Seeker::eval_point(ball_pos, current_point, world_state); + double min_val = Seeker::eval_point(ball_pos, current_point, world_state, field_dimensions); double curr_val{}; // Points in a current_prec radius of the current point, at 45 degree intervals std::vector check_points{ @@ -60,7 +60,7 @@ rj_geometry::Point Seeker::calculate_open_point(double current_prec, double min_ // Finds the best point out of the ones checked for (auto point : check_points) { - curr_val = Seeker::eval_point(ball_pos, point, world_state); + curr_val = Seeker::eval_point(ball_pos, point, world_state, field_dimensions); if (curr_val < min_val) { min_val = curr_val; min = point; @@ -135,7 +135,7 @@ rj_geometry::Point Seeker::correct_point(rj_geometry::Point p, } double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { + const WorldState* world_state, const FieldDimensions& field_dimensions) { // Determines 'how good' a point is // A higher value is a worse point @@ -163,14 +163,20 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren float min_path_dist = 10000; for (auto bot : world_state->their_robots) { rj_geometry::Point opp_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, current_point.dist_to(opp_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); + auto robot_dist = current_point.dist_to(opp_pos); + min_robot_dist = std::min(min_robot_dist, robot_dist); + auto path_dist = pass_path.dist_to(opp_pos); + min_path_dist = std::min(min_path_dist, path_dist); } for (auto bot : world_state->our_robots) { rj_geometry::Point ally_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); + auto robot_dist = current_point.dist_to(ally_pos); + // if dist is 0, then bot must be seeker (self ) robot, so should ignore + if (robot_dist == 0) continue; + min_robot_dist = std::min(min_robot_dist, robot_dist); + auto path_dist = pass_path.dist_to(ally_pos); + min_path_dist = std::min(min_path_dist, path_dist); } min_path_dist = 0.1f / min_path_dist; @@ -187,10 +193,18 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren // Additional heuristics for calculating optimal point double ball_proximity_loss = (current_point - ball_pos).mag() * .002; - double goal_distance_loss = (8.5 - current_point.y()) * 0.15 + abs(current_point.x()) * 0.15; + double goal_distance_loss = (8 - current_point.y()) * 0.15; + + rj_geometry::Segment ball_goal{ball_pos, field_dimensions.their_goal_loc()}; + double block_shot_dist = ball_goal.dist_to(current_point); + double block_shot_loss = 0; + if (block_shot_dist < 1) { + block_shot_loss = 1; + } // Final evaluation - return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; + return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist + + block_shot_loss; } } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/seeker.hpp b/soccer/src/soccer/strategy/agent/position/seeker.hpp index 817f1fed1a6..abdff0a5503 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.hpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.hpp @@ -104,7 +104,8 @@ class Seeker : public RoleInterface { * @return double The evaluation of that target point */ static double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state); + const WorldState* world_state, + const FieldDimensions& field_dimensions); }; } // namespace strategy From eea20964e8266e97f11b2bc4f1560a14d7797523 Mon Sep 17 00:00:00 2001 From: Jack <65086686+jacksherling@users.noreply.github.com> Date: Tue, 5 Mar 2024 19:14:35 -0500 Subject: [PATCH 2/4] Update soccer/src/soccer/strategy/agent/position/seeker.cpp Co-authored-by: Sid Parikh --- soccer/src/soccer/strategy/agent/position/seeker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp index c5aa6f2b754..067c4399e86 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -161,7 +161,7 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren rj_geometry::Segment pass_path{ball_pos, current_point}; double min_robot_dist = 10000; float min_path_dist = 10000; - for (auto bot : world_state->their_robots) { + for (const RobotState& bot : world_state->their_robots) { rj_geometry::Point opp_pos = bot.pose.position(); auto robot_dist = current_point.dist_to(opp_pos); min_robot_dist = std::min(min_robot_dist, robot_dist); From 8871ca2d8f4338d2efd45be070b94727be64e10e Mon Sep 17 00:00:00 2001 From: Jack <65086686+jacksherling@users.noreply.github.com> Date: Tue, 5 Mar 2024 19:15:16 -0500 Subject: [PATCH 3/4] Update soccer/src/soccer/strategy/agent/position/seeker.cpp Co-authored-by: Sid Parikh --- soccer/src/soccer/strategy/agent/position/seeker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp index 067c4399e86..2e2a69d4701 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -37,7 +37,7 @@ rj_geometry::Point Seeker::calculate_open_point(double current_prec, double min_ while (current_prec > min_prec) { rj_geometry::Point ball_pos = world_state->ball.position; rj_geometry::Point min = current_point; - double min_val = Seeker::eval_point(ball_pos, current_point, world_state, field_dimensions); + double min_val = eval_point(ball_pos, current_point, world_state, field_dimensions); double curr_val{}; // Points in a current_prec radius of the current point, at 45 degree intervals std::vector check_points{ From 88eede4f49f2dcd621c40377dfe1bf1cfda6e29f Mon Sep 17 00:00:00 2001 From: jacksherling Date: Tue, 5 Mar 2024 19:32:48 -0500 Subject: [PATCH 4/4] cleanup --- .../soccer/strategy/agent/position/seeker.cpp | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp index 2e2a69d4701..6487344fbfb 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -148,7 +148,7 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren // Line of Sight Heuristic double max = 0; double curr_dp = 0; - for (auto robot : world_state->their_robots) { + for (const RobotState& robot : world_state->their_robots) { curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); curr_dp *= curr_dp; if (curr_dp > max) { @@ -161,19 +161,21 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren rj_geometry::Segment pass_path{ball_pos, current_point}; double min_robot_dist = 10000; float min_path_dist = 10000; - for (const RobotState& bot : world_state->their_robots) { - rj_geometry::Point opp_pos = bot.pose.position(); + for (const RobotState& robot : world_state->their_robots) { + rj_geometry::Point opp_pos = robot.pose.position(); auto robot_dist = current_point.dist_to(opp_pos); min_robot_dist = std::min(min_robot_dist, robot_dist); auto path_dist = pass_path.dist_to(opp_pos); min_path_dist = std::min(min_path_dist, path_dist); } - for (auto bot : world_state->our_robots) { - rj_geometry::Point ally_pos = bot.pose.position(); + for (const RobotState& robot : world_state->our_robots) { + rj_geometry::Point ally_pos = robot.pose.position(); auto robot_dist = current_point.dist_to(ally_pos); // if dist is 0, then bot must be seeker (self ) robot, so should ignore - if (robot_dist == 0) continue; + if (robot_dist == 0) { + continue; + } min_robot_dist = std::min(min_robot_dist, robot_dist); auto path_dist = pass_path.dist_to(ally_pos); min_path_dist = std::min(min_path_dist, path_dist); @@ -183,7 +185,7 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren min_robot_dist = 0.1 / min_robot_dist; // More Line of Sight Heuristics - for (auto robot : world_state->our_robots) { + for (const RobotState& robot : world_state->our_robots) { curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); curr_dp *= curr_dp; if (curr_dp > max) { @@ -192,8 +194,11 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren } // Additional heuristics for calculating optimal point - double ball_proximity_loss = (current_point - ball_pos).mag() * .002; - double goal_distance_loss = (8 - current_point.y()) * 0.15; + const double ball_proximity_loss_weight = 0.002; + const double goal_distance_loss_weight = 0.15; + double ball_proximity_loss = (current_point - ball_pos).mag() * ball_proximity_loss_weight; + double goal_distance_loss = (field_dimensions.their_defense_area().miny() - current_point.y()) * + goal_distance_loss_weight; rj_geometry::Segment ball_goal{ball_pos, field_dimensions.their_goal_loc()}; double block_shot_dist = ball_goal.dist_to(current_point);