diff --git a/src/software/ai/hl/stp/play/play.cpp b/src/software/ai/hl/stp/play/play.cpp index 874972d922..8a400a6e4c 100644 --- a/src/software/ai/hl/stp/play/play.cpp +++ b/src/software/ai/hl/stp/play/play.cpp @@ -22,6 +22,13 @@ Play::Play(std::shared_ptr ai_config_ptr, { halt_tactics.push_back(std::make_shared(ai_config_ptr)); } + previous_robot_tactics.reserve(MAX_ROBOT_IDS_PER_SIDE); + + for (auto& previous_tactic : previous_robot_tactics | std::views::values) + { + previous_tactic.resize(ASSIGNMENTS_CACHE_MAX_SIZE); + } + previous_robot_tactics_count.reserve(MAX_ROBOT_IDS_PER_SIDE); } std::unique_ptr Play::get( @@ -222,12 +229,28 @@ Play::assignTactics(const WorldPtr &world_ptr, TacticVector tactic_vector, { Robot robot = robots_to_assign.at(row); std::shared_ptr tactic = tactic_vector.at(col); + auto primitives = primitive_sets.at(col); CHECK(primitives.contains(robot.id())) << "Couldn't find a primitive for robot id " << robot.id(); double robot_cost_for_tactic = primitives.at(robot.id())->getEstimatedPrimitiveCost(); + + /** + * We want to discourage changing away from a tactic over and over again it is chosen many times + * - penalty should range from 0 to 0.5 + * - store last k assignment matrices + * We can add this flatly to the score + * - score + 0.5 * (# of matching_current)/k + * We can also increase based on a fraction of the current score e.g. + * - score( 1 + 0.15 * (# of matching_current)/k + */ + int previous_tactic_count = previous_robot_tactics_count[robot.id()][std::type_index(typeid(*tactic))]; + double similarity = static_cast(previous_tactic_count) / ASSIGNMENTS_CACHE_MAX_SIZE; + // LOG(DEBUG) << "COUNT:"<< previous_tactic_count <<" SIM:"<< similarity; + robot_cost_for_tactic = std::max(0.0, robot_cost_for_tactic - 0.5 * similarity); + std::set required_capabilities = tactic->robotCapabilityRequirements(); std::set robot_capabilities = @@ -254,6 +277,7 @@ Play::assignTactics(const WorldPtr &world_ptr, TacticVector tactic_vector, // Apply the Munkres/Hungarian algorithm to the matrix. Munkres m; + // LOG(DEBUG) << matrix; m.solve(matrix); // The Munkres matrix gets solved such that there will be exactly one 0 in every @@ -277,6 +301,18 @@ Play::assignTactics(const WorldPtr &world_ptr, TacticVector tactic_vector, robot_id); tactic_vector.at(col)->setLastExecutionRobot(robot_id); + auto tactic = tactic_vector.at(col); + if (previous_robot_tactics[robot_id].size() >= ASSIGNMENTS_CACHE_MAX_SIZE) + { + std::type_index tactic_type = *previous_robot_tactics[robot_id].front(); + previous_robot_tactics_count[robot_id][tactic_type]--; + previous_robot_tactics[robot_id].pop_front(); + + } + std::type_index type = std::type_index(typeid(*tactic)); + previous_robot_tactics[robot_id].push_back(std::make_shared(type)); + previous_robot_tactics_count[robot_id][type]++; + auto primitives = primitive_sets.at(col); CHECK(primitives.contains(robot_id)) << "Couldn't find a primitive for robot id " << robot_id; diff --git a/src/software/ai/hl/stp/play/play.h b/src/software/ai/hl/stp/play/play.h index 92cbd76c3b..40fe2011a5 100644 --- a/src/software/ai/hl/stp/play/play.h +++ b/src/software/ai/hl/stp/play/play.h @@ -2,6 +2,8 @@ #include #include +#include +#include #include "proto/parameters.pb.h" #include "software/ai/hl/stp/play/play_fsm.hpp" @@ -126,4 +128,11 @@ class Play uint64_t sequence_number = 0; RobotNavigationObstacleFactory obstacle_factory; + + static constexpr uint32_t ASSIGNMENTS_CACHE_MAX_SIZE = 20; + + using TacticTypeDeque = std::deque>; + using RobotPreviousTactics = std::unordered_map; + std::unordered_map previous_robot_tactics; + std::unordered_map previous_robot_tactics_count; }; diff --git a/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.cpp b/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.cpp index 216f5293f5..a4a53c875b 100644 --- a/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.cpp @@ -177,6 +177,15 @@ void ReceiverFSM::adjustReceive(const Update& event) TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, AutoChipOrKick{AutoChipOrKickMode::OFF, 0})); } + else + { + event.common.set_primitive(std::make_unique( + event.common.robot, robot_pos, event.common.robot.orientation(), + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, + TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, + AutoChipOrKick{AutoChipOrKickMode::OFF, 0})); + } } bool ReceiverFSM::passStarted(const Update& event) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index eb5bc0bb00..bddc29d75c 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -75,7 +75,7 @@ class EstopMode(IntEnum): ) # How long AI vs AI runs before ending in CI -CI_DURATION_S = 180 +CI_DURATION_S = 600 MULTI_PLANE_POINTS = 3