Speed Calculation in Sim

soccer/src/soccer/strategy/agent/position/offense.cpp

@@ -83,6 +83,7 @@ Offense::State Offense::update_state() {
 }
 
 std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {
+    float dist{0.0f};
     if (current_state_ == IDLING) {
         // Do nothing
         auto empty_motion_cmd = planning::MotionCommand{};
@@ -97,14 +98,19 @@ std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {
         // 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;
         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(FINAL_BALL_SPEED, 2)) - (2 * BALL_DECEL * dist));
         intent.is_active = true;
         return intent;
     } else if (current_state_ == PREPARING_SHOT) {

soccer/src/soccer/strategy/agent/position/offense.hpp

@@ -36,6 +36,9 @@ class Offense : public Position {
 private:
     bool kicking_{true};
 
+    static constexpr float FINAL_BALL_SPEED{0.0f};
+    static constexpr float BALL_DECEL{-0.4f};
+
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
     // TODO (Kevin): strategy design pattern for BallHandler/Receiver