final appraoch plus kick works

soccer/src/soccer/planning/planner/line_kick_path_planner.cpp

@@ -69,37 +69,51 @@ Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) {
     // // Creating a modified plan_request to send to PathTargetPlanner
     PlanRequest modified_request = plan_request;
 
-    // const WorldState* world_state = plan_request.world_state;
-
     // Where to navigate to
     auto distance_from_ball = kBallRadius + kRobotRadius + 0.05;
 
-    auto goal_to_ball = (plan_request.motion_command.target.position - ball.position);
+    auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{0.5}).position;
+
+    auto goal_to_ball = (plan_request.motion_command.target.position - ball_position);
     auto offset_from_ball = distance_from_ball * goal_to_ball.normalized();
 
     // Updating the motion command
-    LinearMotionInstant target{ball.position - offset_from_ball};
+    LinearMotionInstant target{ball_position - offset_from_ball};
 
-    MotionCommand modified_command{"path_target", target, FaceBall{}};
+    MotionCommand modified_command{"path_target", target, FacePoint{plan_request.motion_command.target.position}};
     modified_request.motion_command = modified_command;
 
     // Getting the new path from PathTargetPlanner
     Trajectory path = path_target_.plan(modified_request);
-    // modified_request.motion_command = MotionCommand{"collect"};
 
-    // return collect_planner_.plan(modified_request);
     return path;
 }
 
 Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
-
+    // remove ball from obstacles not needed?
 
+    const BallState& ball = plan_request.world_state->ball;
+
+    // // Creating a modified plan_request to send to PathTargetPlanner
+    PlanRequest modified_request = plan_request;
 
+    //velocity
+    auto goal_to_ball = (plan_request.motion_command.target.position - ball.position);
+    auto vel = goal_to_ball.normalized() * 1.0;
+
+    LinearMotionInstant target{ball.position, vel};
+    MotionCommand modified_command{"path_target", target, FacePoint{plan_request.motion_command.target.position}, true};
+    modified_request.motion_command = modified_command;
+
+    // Getting the new path from PathTargetPlanner
+    Trajectory path = path_target_.plan(modified_request);
+
+    return path;
 }
 
 void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_instant) {
     if (current_state_ == INITIAL_APPROACH &&
-        (ball.position - start_instant.position()).mag() < 0.01) {
+        (path_target_.is_done())) {
         current_state_ = FINAL_APPROACH;
     }
 }

soccer/src/soccer/planning/planner/line_kick_path_planner.hpp

@@ -46,7 +46,7 @@ class LineKickPathPlanner : public PathPlanner {
     CollectPathPlanner collect_planner_{};
     Trajectory prev_path_;
 
-    static constexpr double IS_DONE_BALL_VEL = 0.5;
+    static constexpr double IS_DONE_BALL_VEL = 1.5;
     rj_geometry::Point average_ball_vel_;
     bool average_ball_vel_initialized_ = false;
     std::optional<rj_geometry::Point> target_kick_pos_;
@@ -57,6 +57,7 @@ class LineKickPathPlanner : public PathPlanner {
     // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector<DynamicObstacle> dynamic_obstacles);
     void state_transition(BallState ball, RobotInstant start_instant);
 
+    // PlayState::State current_state_;
 };
 
 }  // namespace planning

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

@@ -127,7 +127,8 @@ std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {
         return intent;
     } else if (current_state_ == SHOOTING) {
         rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc();
-        planning::LinearMotionInstant target{their_goal_pos};
+        rj_geometry::Point scoring_point = their_goal_pos + field_dimensions_.goal_width() / 2.0;
+        planning::LinearMotionInstant target{scoring_point};
         auto line_kick_cmd = planning::MotionCommand{"line_kick", target};
         intent.motion_command = line_kick_cmd;
         intent.shoot_mode = RobotIntent::ShootMode::KICK;