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 <[email protected]> * Resolved conversations on previous PR * Fix Code Style On james-vogt/variable-kick (#2180) automated style fixes Co-authored-by: jvogt23 <[email protected]> --------- Co-authored-by: jvogt23 <[email protected]> Co-authored-by: Sid Parikh <[email protected]> Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: Squid5678 <[email protected]> Co-authored-by: jvogt23 <[email protected]>
RoboJackets · Jan 31, 2024 · cd689b8 · cd689b8
1 parent 26caeb4
commit cd689b8
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 3 deletions.
diff --git 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
@@ -87,8 +87,8 @@ Offense::State Offense::update_state() {
 }
 
 std::optional<RobotIntent> 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<RobotIntent> 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
@@ -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<RobotIntent> derived_get_task(RobotIntent intent) override;
     // TODO (Kevin): strategy design pattern for BallHandler/Receiver