From cd3224b911d033ca1276f3a4dd1dbb323c54bcb6 Mon Sep 17 00:00:00 2001 From: jvogt23 <91633361+jvogt23@users.noreply.github.com> Date: Sun, 3 Dec 2023 19:02:45 -0500 Subject: [PATCH 1/9] Added variable kick speed logic to offense.cpp and offense.hpp, needs tuning --- soccer/src/soccer/strategy/agent/position/offense.cpp | 11 +++++++++-- soccer/src/soccer/strategy/agent/position/offense.hpp | 4 ++++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 812f01bdb83..1cbe9faec0b 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -30,7 +30,7 @@ Offense::State Offense::update_state() { if (current_state_ == IDLING) { send_scorer_request(); - next_state = SEARCHING; + next_state = PASSING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -94,9 +94,13 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == PASSING) { + //TODO: Remove this pls + target_robot_id = 2; // attempt to pass the ball to the target robot rj_geometry::Point target_robot_pos = last_world_state_->get_robot(true, target_robot_id).pose.position(); + rj_geometry::Point this_robot_pos = + last_world_state_->get_robot(true, this->robot_id_).pose.position(); planning::LinearMotionInstant target{target_robot_pos}; auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; intent.motion_command = line_kick_cmd; @@ -104,7 +108,10 @@ std::optional Offense::state_to_task(RobotIntent intent) { // NOTE: Check we can actually use break beams intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; // TODO: Adjust the kick speed based on distance - intent.kick_speed = 4.0; + dist = std::sqrt(std::pow((target_robot_pos.x() - this_robot_pos.x()), 2) + + std::pow((target_robot_pos.y() - this_robot_pos.y()), 2)); + intent.kick_speed = std::sqrt((std::pow(FINAL_BALL_SPEED, 2)) - + (2 * BALL_DECEL * dist)); intent.is_active = true; return intent; } else if (current_state_ == PREPARING_SHOT) { diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index e09b513f192..b79e3297354 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -36,6 +36,10 @@ class Offense : public Position { private: bool kicking_{true}; + const float FINAL_BALL_SPEED {0.0f}; + const float BALL_DECEL {-0.5f}; + float dist {0.0f}; + std::optional derived_get_task(RobotIntent intent) override; // TODO (Kevin): strategy design pattern for BallHandler/Receiver From 9f778ffc197b15e932b659ed9aeea7ee76956ae6 Mon Sep 17 00:00:00 2001 From: jvogt23 <91633361+jvogt23@users.noreply.github.com> Date: Sun, 3 Dec 2023 19:38:12 -0500 Subject: [PATCH 2/9] Undid diagnostic changes --- soccer/src/soccer/strategy/agent/position/offense.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 1cbe9faec0b..cf28bda2451 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -30,7 +30,7 @@ Offense::State Offense::update_state() { if (current_state_ == IDLING) { send_scorer_request(); - next_state = PASSING; + next_state = SEARCHING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -94,8 +94,6 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == PASSING) { - //TODO: Remove this pls - target_robot_id = 2; // attempt to pass the ball to the target robot rj_geometry::Point target_robot_pos = last_world_state_->get_robot(true, target_robot_id).pose.position(); @@ -107,7 +105,8 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.shoot_mode = RobotIntent::ShootMode::KICK; // NOTE: Check we can actually use break beams intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - // TODO: Adjust the kick speed based on distance + //Adjusts kick speed based on distance. Refer to + //TIGERS Mannheim eTDP from 2019 for details. dist = std::sqrt(std::pow((target_robot_pos.x() - this_robot_pos.x()), 2) + std::pow((target_robot_pos.y() - this_robot_pos.y()), 2)); intent.kick_speed = std::sqrt((std::pow(FINAL_BALL_SPEED, 2)) - From da950f9e4e8d626a974f5a434006277cf294a9d3 Mon Sep 17 00:00:00 2001 From: jvogt23 <91633361+jvogt23@users.noreply.github.com> Date: Sun, 3 Dec 2023 19:59:02 -0500 Subject: [PATCH 3/9] Apparently those diagnostic things were needed --- soccer/src/soccer/strategy/agent/position/offense.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index cf28bda2451..3bea2196902 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -30,7 +30,7 @@ Offense::State Offense::update_state() { if (current_state_ == IDLING) { send_scorer_request(); - next_state = SEARCHING; + next_state = PASSING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -94,6 +94,8 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == PASSING) { + //TODO: Remove this + this->target_robot_id = 2; // attempt to pass the ball to the target robot rj_geometry::Point target_robot_pos = last_world_state_->get_robot(true, target_robot_id).pose.position(); From 11b737895f1e860f008483685921fd3aa8aa02d2 Mon Sep 17 00:00:00 2001 From: Squid5678 Date: Mon, 4 Dec 2023 01:23:52 +0000 Subject: [PATCH 4/9] Tuned values, cleaned up code --- framework | 1 + soccer/src/soccer/strategy/agent/position/offense.cpp | 7 +++++-- soccer/src/soccer/strategy/agent/position/offense.hpp | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) create mode 160000 framework diff --git a/framework b/framework new file mode 160000 index 00000000000..0f5ffffa441 --- /dev/null +++ b/framework @@ -0,0 +1 @@ +Subproject commit 0f5ffffa4417be583357c923ee4217183ed5c260 diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 3bea2196902..9cee4c3f38c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -30,7 +30,7 @@ Offense::State Offense::update_state() { if (current_state_ == IDLING) { send_scorer_request(); - next_state = PASSING; + next_state = SEARCHING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -95,7 +95,10 @@ std::optional Offense::state_to_task(RobotIntent intent) { return intent; } else if (current_state_ == PASSING) { //TODO: Remove this - this->target_robot_id = 2; + /* + Commented for now, may need for testing later. + */ + //this->target_robot_id = 2; // attempt to pass the ball to the target robot rj_geometry::Point target_robot_pos = last_world_state_->get_robot(true, target_robot_id).pose.position(); diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index b79e3297354..b31979daf11 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -37,7 +37,7 @@ class Offense : public Position { bool kicking_{true}; const float FINAL_BALL_SPEED {0.0f}; - const float BALL_DECEL {-0.5f}; + const float BALL_DECEL {-0.4f}; float dist {0.0f}; std::optional derived_get_task(RobotIntent intent) override; From d72c88bd54faf18ca852a1b16f1d32c38efbd2d7 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Tue, 9 Jan 2024 21:00:36 -0500 Subject: [PATCH 5/9] Abstracted code and review with Sid. Added refs --- soccer/src/soccer/strategy/agent/position/offense.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 9cee4c3f38c..c625ce611fc 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -111,9 +111,9 @@ std::optional Offense::state_to_task(RobotIntent intent) { // NOTE: Check we can actually use break beams intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; //Adjusts kick speed based on distance. Refer to - //TIGERS Mannheim eTDP from 2019 for details. - dist = std::sqrt(std::pow((target_robot_pos.x() - this_robot_pos.x()), 2) + - std::pow((target_robot_pos.y() - this_robot_pos.y()), 2)); + //TIGERS Mannheim eTDP from 2019 for details + //See also passer.py in rj_gameplay + dist = target_robot_pos.dist_to(this_robot_pos); intent.kick_speed = std::sqrt((std::pow(FINAL_BALL_SPEED, 2)) - (2 * BALL_DECEL * dist)); intent.is_active = true; From 221bf56385a1006604f367055a2a430d987db037 Mon Sep 17 00:00:00 2001 From: Squid5678 Date: Mon, 15 Jan 2024 00:58:58 +0000 Subject: [PATCH 6/9] Addressed changes requested by Sid --- soccer/src/soccer/strategy/agent/position/offense.cpp | 6 +----- soccer/src/soccer/strategy/agent/position/offense.hpp | 6 +++--- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index c625ce611fc..524ad177756 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -83,6 +83,7 @@ Offense::State Offense::update_state() { } std::optional Offense::state_to_task(RobotIntent intent) { + float dist {0.0f}; if (current_state_ == IDLING) { // Do nothing auto empty_motion_cmd = planning::MotionCommand{}; @@ -94,11 +95,6 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == PASSING) { - //TODO: Remove this - /* - Commented for now, may need for testing later. - */ - //this->target_robot_id = 2; // attempt to pass the ball to the target robot rj_geometry::Point target_robot_pos = last_world_state_->get_robot(true, target_robot_id).pose.position(); diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index b31979daf11..053f230149f 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -36,9 +36,9 @@ class Offense : public Position { private: bool kicking_{true}; - const float FINAL_BALL_SPEED {0.0f}; - const float BALL_DECEL {-0.4f}; - float dist {0.0f}; + static constexpr float FINAL_BALL_SPEED {0.0f}; + static constexpr float BALL_DECEL {-0.4f}; + std::optional derived_get_task(RobotIntent intent) override; // TODO (Kevin): strategy design pattern for BallHandler/Receiver From 30f4eb973e1573c5d6e81e7a814f828922ca172f Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 21 Jan 2024 20:52:39 -0500 Subject: [PATCH 7/9] Fix Code Style On james-vogt/variable-kick (#2156) automated style fixes Co-authored-by: Squid5678 --- soccer/src/soccer/strategy/agent/position/offense.cpp | 11 +++++------ soccer/src/soccer/strategy/agent/position/offense.hpp | 5 ++--- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 524ad177756..a819d466f76 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -83,7 +83,7 @@ Offense::State Offense::update_state() { } std::optional Offense::state_to_task(RobotIntent intent) { - float dist {0.0f}; + float dist{0.0f}; if (current_state_ == IDLING) { // Do nothing auto empty_motion_cmd = planning::MotionCommand{}; @@ -106,12 +106,11 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.shoot_mode = RobotIntent::ShootMode::KICK; // NOTE: Check we can actually use break beams intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - //Adjusts kick speed based on distance. Refer to - //TIGERS Mannheim eTDP from 2019 for details - //See also passer.py in rj_gameplay + // Adjusts kick speed based on distance. Refer to + // TIGERS Mannheim eTDP from 2019 for details + // See also passer.py in rj_gameplay dist = target_robot_pos.dist_to(this_robot_pos); - intent.kick_speed = std::sqrt((std::pow(FINAL_BALL_SPEED, 2)) - - (2 * BALL_DECEL * dist)); + intent.kick_speed = std::sqrt((std::pow(FINAL_BALL_SPEED, 2)) - (2 * BALL_DECEL * dist)); intent.is_active = true; return intent; } else if (current_state_ == PREPARING_SHOT) { diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 053f230149f..4d778c9beba 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -36,9 +36,8 @@ class Offense : public Position { private: bool kicking_{true}; - static constexpr float FINAL_BALL_SPEED {0.0f}; - static constexpr float BALL_DECEL {-0.4f}; - + static constexpr float FINAL_BALL_SPEED{0.0f}; + static constexpr float BALL_DECEL{-0.4f}; std::optional derived_get_task(RobotIntent intent) override; // TODO (Kevin): strategy design pattern for BallHandler/Receiver From 8c6271da852a0e05665d54e102241d601e45a57a Mon Sep 17 00:00:00 2001 From: jvogt23 <91633361+jvogt23@users.noreply.github.com> Date: Sun, 28 Jan 2024 21:31:55 -0500 Subject: [PATCH 8/9] Resolved conversations on previous PR --- framework | 1 - rj_constants/include/rj_constants/constants.hpp | 3 +++ soccer/src/soccer/strategy/agent/position/offense.cpp | 2 +- soccer/src/soccer/strategy/agent/position/offense.hpp | 5 +++-- 4 files changed, 7 insertions(+), 4 deletions(-) delete mode 160000 framework diff --git a/framework b/framework deleted file mode 160000 index 0f5ffffa441..00000000000 --- a/framework +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0f5ffffa4417be583357c923ee4217183ed5c260 diff --git a/rj_constants/include/rj_constants/constants.hpp b/rj_constants/include/rj_constants/constants.hpp index fb83fd3bde0..a96b19e63cb 100644 --- a/rj_constants/include/rj_constants/constants.hpp +++ b/rj_constants/include/rj_constants/constants.hpp @@ -25,6 +25,9 @@ constexpr float kRobotHeight = 0.150f; constexpr float kRobotMouthWidth = 0.0635f; constexpr float kRobotMouthRadius = 0.078f; +// Constant for ball deceleration on field +constexpr float kBallDecel {-0.4f}; + /** constants for dot patterns */ constexpr float kDotsSmallOffset = 0.035; constexpr float kDotsLargeOffset = 0.054772; diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index a819d466f76..6ceb062dddd 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -110,7 +110,7 @@ std::optional Offense::state_to_task(RobotIntent intent) { // TIGERS Mannheim eTDP from 2019 for details // See also passer.py in rj_gameplay dist = target_robot_pos.dist_to(this_robot_pos); - intent.kick_speed = std::sqrt((std::pow(FINAL_BALL_SPEED, 2)) - (2 * BALL_DECEL * dist)); + intent.kick_speed = std::sqrt((std::pow(kFinalBallSpeed, 2)) - (2 * kBallDecel * dist)); intent.is_active = true; return intent; } else if (current_state_ == PREPARING_SHOT) { diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 4d778c9beba..fb30b0a660d 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -11,6 +11,7 @@ #include "planning/instant.hpp" #include "position.hpp" #include "rj_common/time.hpp" +#include "rj_constants/constants.hpp" #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" @@ -36,8 +37,8 @@ class Offense : public Position { private: bool kicking_{true}; - static constexpr float FINAL_BALL_SPEED{0.0f}; - static constexpr float BALL_DECEL{-0.4f}; + //These variables are for calculating ball speed when passing + static constexpr float kFinalBallSpeed {0.0f}; std::optional derived_get_task(RobotIntent intent) override; // TODO (Kevin): strategy design pattern for BallHandler/Receiver From 606a76ff4a3ba82233bb8a3884a18407e5fa3d67 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 29 Jan 2024 18:04:11 -0500 Subject: [PATCH 9/9] Fix Code Style On james-vogt/variable-kick (#2180) automated style fixes Co-authored-by: jvogt23 --- rj_constants/include/rj_constants/constants.hpp | 2 +- soccer/src/soccer/strategy/agent/position/offense.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rj_constants/include/rj_constants/constants.hpp b/rj_constants/include/rj_constants/constants.hpp index 25bb49c241f..8a11ff79f79 100644 --- a/rj_constants/include/rj_constants/constants.hpp +++ b/rj_constants/include/rj_constants/constants.hpp @@ -26,7 +26,7 @@ constexpr float kRobotMouthWidth = 0.0635f; constexpr float kRobotMouthRadius = 0.078f; // Constant for ball deceleration on field -constexpr float kBallDecel {-0.4f}; +constexpr float kBallDecel{-0.4f}; /** constants for dot patterns */ constexpr float kDotsSmallOffset = 0.035; diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 160f792ac0b..0bb1f5e641d 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -37,8 +37,8 @@ class Offense : public Position { private: bool kicking_{true}; - //These variables are for calculating ball speed when passing - static constexpr float kFinalBallSpeed {0.0f}; + // These variables are for calculating ball speed when passing + static constexpr float kFinalBallSpeed{0.0f}; std::optional derived_get_task(RobotIntent intent) override; // TODO (Kevin): strategy design pattern for BallHandler/Receiver