Skip to content

Commit

Permalink
Cameron carson/penalty position dribbling (#2336)
Browse files Browse the repository at this point in the history 
* 1/26/2025

* 2/2/2025, modified behaviour of penalty shooting so that the ball gets kicked once shortly, follows it, then kicks to goal. Only works when ball is at center field.

* Modified penalty behavior to work from the correct position (y=3). Follows ball by kicking with speed 0 and then kicks when closer to the goal. WARNING: Has only been tested on yellow side

* quick edit to the .hpp states

* 2/4/2025, added improvements following Jack's comments

* quick change to a single line

* Fix Code Style On cameron-carson/penalty-position-dribbling (#2337)

automated style fixes

Co-authored-by: CameronLyon <[email protected]>

* Edits based on pull request comments.
Mainly cleaned up comments and fixed inconsistencies in variable naming- improved code readability

* Fix Code Style On cameron-carson/penalty-position-dribbling (#2345)

automated style fixes

Co-authored-by: rociopv06 <[email protected]>

---------

Co-authored-by: rociopv06 <[email protected]>
Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: CameronLyon <[email protected]>
Co-authored-by: rociopv06 <[email protected]>
  • Loading branch information
@CameronLyon @rociopv06 @github-actions
5 people authored Feb 19, 2025
1 parent 298910d commit 0729903
Show file tree
Hide file tree
Showing 2 changed files with 74 additions and 19 deletions.
81 changes: 64 additions & 17 deletions soccer/src/soccer/strategy/agent/position/penalty_player.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,22 @@ std::optional<RobotIntent> PenaltyPlayer::derived_get_task(RobotIntent intent) {

PenaltyPlayer::State PenaltyPlayer::update_state() {
switch (latest_state_) {
case LINE_UP: {
case START: {
// if penalty playing and restart penalty in playstate we switch to shooting
if (current_play_state_.is_ready() &&
(current_play_state_.is_penalty() || current_play_state_.is_kickoff())) {
return SMALL_KICK;
}
break;
}
case SMALL_KICK: {
if (distance_from_enemy_goal() < kDistanceToGoalThreshold) {
return LINE_UP;
}
break;
}
case LINE_UP: {
if (check_is_done() || distance_to_ball() < kOwnBallRadius) {
return SHOOTING_START;
}
break;
Expand All @@ -26,14 +38,12 @@ PenaltyPlayer::State PenaltyPlayer::update_state() {
if (check_is_done()) {
return SHOOTING;
}
if (distance_to_ball() < kOwnBallRadius) {
return SHOOTING;
}

break;
}
case SHOOTING: {
if (check_is_done()) {
return LINE_UP;
return START;
}
break;
}
Expand All @@ -43,14 +53,40 @@ PenaltyPlayer::State PenaltyPlayer::update_state() {

std::optional<RobotIntent> PenaltyPlayer::state_to_task(RobotIntent intent) {
switch (latest_state_) {
case LINE_UP: {
case START: { // First, gets the robot to the ball to begin penalty dribbling-shooting
double y_pos = last_world_state_->ball.position.y();
// if ball is above goal, increase y_pos, else decrease
// if (y_pos - field_dimensions_.their_goal_loc().y() > 0) {
// y_pos += kRobotRadius - 0.15;
// } else {
// Add 0.3 buffer space to the y_pos of the ball to ensure the robot does not
// hit the ball before being properly lined up behind it
y_pos -= kRobotRadius + 0.3;
// }
rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos};
rj_geometry::Point target_vel{0.0, 0.0};
// Face ball
planning::PathTargetFaceOption face_option{planning::FaceBall{}};

// Create Motion Command
planning::LinearMotionInstant goal{target_pt, target_vel};
intent.motion_command =
planning::MotionCommand{"path_target", goal, planning::FaceBall{}};
return intent;
}
case SMALL_KICK: { // less of a kick, more of a "follow" ball closely
rj_geometry::Point center_goal = field_dimensions_.their_goal_loc();
auto line_kick_cmd =
planning::MotionCommand{"line_kick", planning::LinearMotionInstant{center_goal}};

intent.motion_command = line_kick_cmd;
intent.shoot_mode = RobotIntent::ShootMode::KICK;
intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;
intent.kick_speed = 0.0;
// The point of making a 0 kick speed is to fake dribble since we cannot get the vaccum
// behavior to work

return intent;
}
case LINE_UP: { // Gets the robot behind the ball with a certain distance (usually
// immediatly skipped if the robot is already close)
double y_pos = last_world_state_->ball.position.y();
y_pos -= kOwnBallRadius;
rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos};
rj_geometry::Point target_vel{0.0, 0.0};
// Face ball
Expand All @@ -62,9 +98,11 @@ std::optional<RobotIntent> PenaltyPlayer::state_to_task(RobotIntent intent) {
planning::LinearMotionInstant goal{target_pt, target_vel};
intent.motion_command =
planning::MotionCommand{"path_target", goal, face_option, ignore_ball};
break;

return intent;
}
case SHOOTING_START: {
case SHOOTING_START: { // Positions the robot behind the ball at a certain angle so that it
// has a straight shot towards the goal
target_ = calculate_best_shot();
rj_geometry::Point ball_position = last_world_state_->ball.position;
auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position();
Expand All @@ -77,7 +115,8 @@ std::optional<RobotIntent> PenaltyPlayer::state_to_task(RobotIntent intent) {

return intent;
}
case SHOOTING: {
case SHOOTING: { // Kicks the ball with a now much higher speed (basically SMALL_KICK but
// power set to 4)
auto line_kick_cmd =
planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}};

Expand All @@ -87,7 +126,6 @@ std::optional<RobotIntent> PenaltyPlayer::state_to_task(RobotIntent intent) {
intent.kick_speed = 4.0;

return intent;
break;
}
}

Expand Down Expand Up @@ -120,20 +158,29 @@ double PenaltyPlayer::distance_from_their_robots(rj_geometry::Point tail,
}
return min_angle;
}

/**
* @brief Iterates across 19 possible shot target locations along the goal width in 0.05-meter
* increments. For each location, it calculates the clearance distance from opponent robots and
* updates the best shot position if a better (less obstructed) option is found.
* @return The best target position (farthest from obstacles) found after considering all 19
* possibilities.
*/
rj_geometry::Point PenaltyPlayer::calculate_best_shot() const {
// Goal location
rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc();
// TODO: Consider reducing goal width variable to reduce the possibility of the shot missing at
// edges
double goal_width = field_dimensions_.goal_width(); // 1.0 meters

// Ball location
rj_geometry::Point ball_position = this->last_world_state_->ball.position;

// Sets initial target shot at the middle of their goal
rj_geometry::Point best_shot = their_goal_pos;
double best_distance = -1.0;
rj_geometry::Point increment(0.05, 0);
rj_geometry::Point curr_point =
their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment;
// Compare shots to find best possibility
for (int i = 0; i < 19; i++) {
double distance = distance_from_their_robots(ball_position, curr_point);
if (distance > best_distance) {
Expand Down
12 changes: 10 additions & 2 deletions soccer/src/soccer/strategy/agent/position/penalty_player.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,12 @@ class PenaltyPlayer : public Position {
private:
std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;

enum State { LINE_UP, SHOOTING_START, SHOOTING };
enum State { START, SMALL_KICK, LINE_UP, SHOOTING_START, SHOOTING };

static constexpr double kOwnBallRadius{kRobotRadius + 0.1};

static constexpr double kDistanceToGoalThreshold{3.5};

State update_state();

/**
Expand All @@ -56,6 +58,12 @@ class PenaltyPlayer : public Position {
last_world_state_->get_robot(true, robot_id_).pose.position());
};

double distance_from_enemy_goal() const {
return last_world_state_->get_robot(true, robot_id_)
.pose.position()
.dist_to(field_dimensions_.their_goal_loc());
};

/**
* @return the target (within the goal) that would be the most clear shot
*/
Expand All @@ -77,7 +85,7 @@ class PenaltyPlayer : public Position {
std::optional<RobotIntent> state_to_task(RobotIntent intent);

// current state of Goalie (state machine)
State latest_state_ = LINE_UP;
State latest_state_ = START;
};

} // namespace strategy

0 comments on commit 0729903

Please sign in to comment.