From cd689b8825f9f3c43129275b9f1453e7a06f0122 Mon Sep 17 00:00:00 2001 From: Aalind Tyagi <42896179+Squid5678@users.noreply.github.com> Date: Wed, 31 Jan 2024 00:17:55 +0000 Subject: [PATCH] Speed Calculation in Sim (#2153) * Added variable kick speed logic to offense.cpp and offense.hpp, needs tuning * Undid diagnostic changes * Apparently those diagnostic things were needed * Tuned values, cleaned up code * Abstracted code and review with Sid. Added refs * Addressed changes requested by Sid * Fix Code Style On james-vogt/variable-kick (#2156) automated style fixes Co-authored-by: Squid5678 * Resolved conversations on previous PR * Fix Code Style On james-vogt/variable-kick (#2180) automated style fixes Co-authored-by: jvogt23 --------- Co-authored-by: jvogt23 <91633361+jvogt23@users.noreply.github.com> Co-authored-by: Sid Parikh Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: Squid5678 Co-authored-by: jvogt23 --- rj_constants/include/rj_constants/constants.hpp | 3 +++ soccer/src/soccer/strategy/agent/position/offense.cpp | 11 ++++++++--- soccer/src/soccer/strategy/agent/position/offense.hpp | 4 ++++ 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/rj_constants/include/rj_constants/constants.hpp b/rj_constants/include/rj_constants/constants.hpp index 199101e80c1..8a11ff79f79 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 689e50eda7d..017eca53a11 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -87,8 +87,8 @@ Offense::State Offense::update_state() { } std::optional Offense::state_to_task(RobotIntent intent) { + float dist{0.0f}; SPDLOG_INFO(current_state_); - if (current_state_ == IDLING) { // Do nothing auto empty_motion_cmd = planning::MotionCommand{}; @@ -103,14 +103,19 @@ std::optional Offense::state_to_task(RobotIntent intent) { target_robot_id = 2; 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; 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 - intent.kick_speed = 4.0; + // 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(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 2c13dacef8f..0bb1f5e641d 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,6 +37,9 @@ class Offense : public Position { private: bool kicking_{true}; + // 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