Stopplay intelligent positioning fsm #3630
+347
−106
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,104 @@ | ||
| #include "software/ai/hl/stp/play/stop_play_fsm.h" | ||
|
|
||
| #include "shared/constants.h" | ||
|
|
||
| StopPlayFSM::StopPlayFSM(std::shared_ptr<const TbotsProto::AiConfig> ai_config_ptr) | ||
| : ai_config_ptr(ai_config_ptr), | ||
| move_tactics{std::make_shared<MoveTactic>(ai_config_ptr), | ||
| std::make_shared<MoveTactic>(ai_config_ptr), | ||
| std::make_shared<MoveTactic>(ai_config_ptr)}, | ||
| crease_defender_tactics{std::make_shared<CreaseDefenderTactic>(ai_config_ptr), | ||
| std::make_shared<CreaseDefenderTactic>(ai_config_ptr)} | ||
| { | ||
| } | ||
|
|
||
| void StopPlayFSM::updateStopPosition(const Update& event) | ||
| { | ||
| // Robot assignments for the Stop Play | ||
| // - 1 robot will be the goalie | ||
| // - 2 robots will assist the goalie in blocking the ball, they will snap | ||
| // to the best fit semicircle around the defense area | ||
| // - 2 robots will stay within 0.5m of the ball, curved around the goal-to-ball | ||
| // line | ||
| // - 1 robot will position more centrally in our half to help stretch the field | ||
| // | ||
| // If x represents the ball and G represents the goalie, the following diagram | ||
| // depicts a possible outcome of this play | ||
| // | ||
| // +--------------------+--------------------+ | ||
| // | | | | ||
| // | 4 x | | | ||
| // | 0 2 | | | ||
| // +--+ 1 | +--+ | ||
| // | | | | | | ||
| // |G | +-+-+ | | | ||
| // | | | | | | | ||
| // | | +-+-+ | | | ||
| // | | 3 | | | | ||
| // +--+ | +--+ | ||
| // | | | | ||
| // | | | | ||
| // | | | | ||
| // +--------------------+--------------------+ | ||
| const WorldPtr& world_ptr = event.common.world_ptr; | ||
| TbotsProto::MaxAllowedSpeedMode stop_mode = | ||
| TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND; | ||
|
|
||
| // A unit vector from the center of the goal to the ball; used for | ||
| // positioning all non-goalie robots. The perpendicular is used to place | ||
| // robots tangent to the goal-to-ball line. | ||
| Vector goal_to_ball_unit_vector = | ||
| (world_ptr->field().friendlyGoalCenter() - world_ptr->ball().position()) | ||
| .normalize(); | ||
| Vector robot_positioning_unit_vector = goal_to_ball_unit_vector.perpendicular(); | ||
|
|
||
| // Points on the circle around the ball: center on the goal-ball line, | ||
| // and one offset. Extra robot radius buffer to stay within rules. | ||
| Point ball_defense_point_center = | ||
| world_ptr->ball().position() + | ||
| (0.5 + 2 * ROBOT_MAX_RADIUS_METERS) * goal_to_ball_unit_vector; | ||
| Point ball_defense_point_right = | ||
| ball_defense_point_center + | ||
| robot_positioning_unit_vector * 4 * ROBOT_MAX_RADIUS_METERS; | ||
annieisawesome2 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
|
||
| // A spread-out position for the support robot: we take the point along the | ||
| // goal-to-ball line at CENTRAL_SUPPORT_FRACTION, then mirror it across the | ||
| // field center so the support robot is on the opposite side of the field | ||
| // from the ball (stretching the field). | ||
| constexpr double CENTRAL_SUPPORT_FRACTION = 0.5; | ||
|
Contributor
There was a problem hiding this comment. Put this in header file |
||
| Point point_along_goal_ball = | ||
| world_ptr->field().friendlyGoalCenter() + | ||
| (world_ptr->ball().position() - world_ptr->field().friendlyGoalCenter()) * | ||
| CENTRAL_SUPPORT_FRACTION; | ||
|
|
||
| Point field_center = world_ptr->field().centerPoint(); | ||
| Point central_support_point = field_center + (field_center - point_along_goal_ball); | ||
|
|
||
| move_tactics.at(0)->updateControlParams( | ||
| ball_defense_point_center, | ||
| (world_ptr->ball().position() - ball_defense_point_center).orientation(), | ||
| stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); | ||
| move_tactics.at(1)->updateControlParams( | ||
| ball_defense_point_right, | ||
| (world_ptr->ball().position() - ball_defense_point_right).orientation(), | ||
| stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); | ||
| move_tactics.at(2)->updateControlParams( | ||
| central_support_point, | ||
| (world_ptr->ball().position() - central_support_point).orientation(), stop_mode, | ||
| TbotsProto::ObstacleAvoidanceMode::SAFE); | ||
|
|
||
| std::get<0>(crease_defender_tactics) | ||
| ->updateControlParams(world_ptr->ball().position(), | ||
| TbotsProto::CreaseDefenderAlignment::LEFT, stop_mode, | ||
| TbotsProto::BallStealMode::IGNORE); | ||
| std::get<1>(crease_defender_tactics) | ||
| ->updateControlParams(world_ptr->ball().position(), | ||
| TbotsProto::CreaseDefenderAlignment::RIGHT, stop_mode, | ||
| TbotsProto::BallStealMode::IGNORE); | ||
|
|
||
| PriorityTacticVector result = {{}}; | ||
| result[0].emplace_back(std::get<0>(crease_defender_tactics)); | ||
| result[0].emplace_back(std::get<1>(crease_defender_tactics)); | ||
| result[0].insert(result[0].end(), move_tactics.begin(), move_tactics.end()); | ||
| event.common.set_tactics(result); | ||
| } | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,62 @@ | ||
| #pragma once | ||
|
|
||
| #include <memory> | ||
|
|
||
| #include "proto/parameters.pb.h" | ||
| #include "shared/constants.h" | ||
| #include "software/ai/hl/stp/play/play_fsm.hpp" | ||
| #include "software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic.h" | ||
| #include "software/ai/hl/stp/tactic/move/move_tactic.h" | ||
|
|
||
| struct StopPlayFSM | ||
| { | ||
| struct ControlParams | ||
| { | ||
| }; | ||
| class StopState; | ||
|
|
||
| struct Update | ||
| { | ||
| Update(const ControlParams& control_params, const PlayUpdate& common) | ||
| : control_params(control_params), common(common) | ||
| { | ||
| } | ||
| ControlParams control_params; | ||
| PlayUpdate common; | ||
| }; | ||
|
|
||
| /** | ||
| * Creates a Stop Play FSM | ||
| * | ||
| * @param ai_config_ptr shared pointer to the play config for this FSM | ||
| */ | ||
| explicit StopPlayFSM(std::shared_ptr<const TbotsProto::AiConfig> ai_config_ptr); | ||
|
|
||
| /** | ||
| * Action to position robots during Stop (goalie handled by Play; this sets | ||
| * crease defenders and robots near the ball). | ||
| * | ||
| * @param event the StopPlayFSM Update event | ||
| */ | ||
| void updateStopPosition(const Update& event); | ||
|
|
||
| auto operator()() | ||
| { | ||
| using namespace boost::sml; | ||
|
|
||
| DEFINE_SML_STATE(StopState) | ||
|
|
||
| DEFINE_SML_EVENT(Update) | ||
|
|
||
| DEFINE_SML_ACTION(updateStopPosition) | ||
|
|
||
| return make_transition_table( | ||
| // src_state + event [guard] / action = dest_state | ||
| *StopState_S + Update_E / updateStopPosition_A = StopState_S); | ||
| } | ||
|
|
||
| private: | ||
| std::shared_ptr<const TbotsProto::AiConfig> ai_config_ptr; | ||
| std::vector<std::shared_ptr<MoveTactic>> move_tactics; | ||
| std::array<std::shared_ptr<CreaseDefenderTactic>, 2> crease_defender_tactics; | ||
| }; |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
not sure where this came from.. had a divergent branch and did a merge..