Split Link Kick into 2 Planners

soccer/src/soccer/CMakeLists.txt

@@ -37,6 +37,8 @@ set(ROBOCUP_LIB_SRC
     planning/planner/escape_obstacles_path_planner.cpp
     planning/planner/intercept_path_planner.cpp
     planning/planner/line_kick_path_planner.cpp
+    planning/planner/line_kick_planner_one.cpp
+    planning/planner/line_kick_planner_two.cpp
     planning/planner/path_target_path_planner.cpp
     planning/planner/pivot_path_planner.cpp
     planning/planner/plan_request.cpp

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

@@ -67,11 +67,11 @@
 
     LinearMotionInstant target{ball_position - offset_from_ball};
 
-    MotionCommand modified_command{"path_target", target,
+    MotionCommand modified_command{"initial_path_target", target,
                                    FacePoint{plan_request.motion_command.target.position}};
     modified_request.motion_command = modified_command;
 
-    return path_target_.plan(modified_request);
+    return initial_planner.plan(modified_request);
 }
 
 Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
@@ -86,17 +86,17 @@
 
     LinearMotionInstant target{ball.position, vel};
 
-    MotionCommand modified_command{"path_target", target,
+    MotionCommand modified_command{"final_path_target", target,
                                    FacePoint{plan_request.motion_command.target.position}};
     modified_request.motion_command = modified_command;
 
-    return path_target_.plan(modified_request);
+    return final_planner.plan(modified_request);
 }
 
 void LineKickPathPlanner::process_state_transition() {
     // Let PathTarget decide when the first stage is done
     // Possible problem: can PathTarget get stuck and loop infinitely?
-    if (current_state_ == INITIAL_APPROACH && path_target_.is_done()) {
+    if (current_state_ == INITIAL_APPROACH && initial_planner.is_done()) {
         current_state_ = FINAL_APPROACH;
     }
 }

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

@@ -45,7 +45,8 @@
 
     State current_state_{INITIAL_APPROACH};
 
-    PathTargetPathPlanner path_target_{};
+    PathTargetPathPlanner initial_planner_{};
+    PathTargetPathPlanner final_planner{};
     CollectPathPlanner collect_planner_{};
     Trajectory prev_path_;
 

soccer/src/soccer/planning/planner/line_kick_planner_one.cpp

@@ -0,0 +1,80 @@
+#include "planning/planner/line_kick_planner_one.hpp"
+
+#include <rj_geometry/util.hpp>
+
+#include "control/trapezoidal_motion.hpp"
+#include "escape_obstacles_path_planner.hpp"
+#include "planning/primitives/create_path.hpp"
+#include "planning/trajectory_utils.hpp"
+
+using namespace std;
+using namespace rj_geometry;
+
+namespace planning {
+
+Trajectory LineKickPlannerOne::plan(const PlanRequest& plan_request) {
+    // If we are not allowed to touch the ball, this planner always fails
+    // This is preferred to simply ending the planner because it is possible (likely)
+    // that strategy re-requests the same planner anyway.
+    if (plan_request.play_state == PlayState::halt() ||
+        plan_request.play_state == PlayState::stop()) {
+        return Trajectory{};
+    }
+
+    latest_robot_pos_ = plan_request.start.pose.position();
+
+    const BallState& ball = plan_request.world_state->ball;
+
+    latest_ball_state_ = ball;
+
+    if (!average_ball_vel_initialized_) {
+        average_ball_vel_ = ball.velocity;
+        average_ball_vel_initialized_ = true;
+    } else {
+        // Add the newest ball velocity measurement to the average velocity
+        // estimate, but downweight the new value heavily
+        //
+        // e.g. new_avg_vel = (0.8 * avg_vel) + (0.2 * new_vel)
+        //
+        average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8);
+    }
+
+    prev_path_ = initial(plan_request);
+
+    prev_path_.stamp(RJ::now());
+    return prev_path_;
+}
+
+Trajectory LineKickPlannerOne::initial(const PlanRequest& plan_request) {
+    // Getting ball info
+    const BallState& ball = plan_request.world_state->ball;
+
+    // Distance to stay away from the ball
+    auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy * 4;
+
+    // In case the ball is (slowly) moving
+    auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position;
+
+    // Along the vector from the goal to ball
+    auto goal_to_ball = (plan_request.motion_command.target.position - ball_position);
+    auto offset_from_ball = distance_from_ball * goal_to_ball.normalized();
+
+    // Create an updated MotionCommand and forward to PathTargetPathPlaner
+    PlanRequest modified_request = plan_request;
+
+    LinearMotionInstant target{ball_position - offset_from_ball};
+
+    MotionCommand modified_command{"path_target", target,
+                                   FacePoint{plan_request.motion_command.target.position}};
+    modified_request.motion_command = modified_command;
+
+    return path_target_.plan(modified_request);
+}
+
+bool LineKickPlannerOne::is_done() const {
+    // end when close enough to the ball
+
+    return path_target_.is_done();
+}
+
+}  // namespace planning

soccer/src/soccer/planning/planner/line_kick_planner_one.hpp

@@ -0,0 +1,65 @@
+#pragma once
+
+#include <optional>
+
+#include "planning/planner/path_planner.hpp"
+#include "planning/planner/path_target_path_planner.hpp"
+#include "planning/trajectory.hpp"
+
+namespace planning {
+
+/**
+ * PathPlanner which plans a path to line kick a ball.
+ * It takes the ball's current trajectory and plans a path for the robot
+ * to intersect with the ball.
+ *
+ * It is in two stages. The first stage drives up to the ball, trying to avoid
+ * accidentally tapping it out of place. The second stage drives directly
+ * into the ball, hopefully kicking it if the hardware has been instructed
+ * to kick.
+ *
+ * Because of the two-stage system, this planner is *stateful*. It ought
+ * not to be destructed and re-constructed during a single execution;
+ * the best approach is to call plan() on each tick.
+ * However, it also shouldn't *completely* break if it is reset.
+ *
+ * Params taken from MotionCommand:
+ *   target.position - planner will kick to this point
+ */
+class LineKickPlannerOne : public PathPlanner {
+public:
+    LineKickPlannerOne() : PathPlanner("line_kick_one"){};
+    Trajectory plan(const PlanRequest& plan_request) override;
+
+    void reset() override {
+        prev_path_ = {};
+        average_ball_vel_initialized_ = false;
+    }
+
+    [[nodiscard]] bool is_done() const override;
+
+private:
+    PathTargetPathPlanner path_target_{};
+    Trajectory prev_path_;
+    rj_geometry::Point latest_robot_pos_;
+
+    // These constants could be tuned more
+    static constexpr double kIsDoneBallVel{1.5};
+    static constexpr double kFinalRobotSpeed{1.0};
+    static constexpr double kPredictIn{0.5};  // seconds
+    static constexpr double kAvoidBallBy{0.05};
+    static constexpr double kLowPassFilterGain{0.2};
+
+    rj_geometry::Point average_ball_vel_;
+    BallState latest_ball_state_;
+    bool average_ball_vel_initialized_ = false;
+
+    /**
+     * Returns the trajectory during the initial stage.
+     * Uses PathTargetPathPlanner to draw a path to the spot to kick from.
+     * Avoids the ball
+     */
+    Trajectory initial(const PlanRequest& plan_request);
+};
+
+}  // namespace planning

soccer/src/soccer/planning/planner/line_kick_planner_two.cpp

@@ -0,0 +1,68 @@
+#include "planning/planner/line_kick_planner_two.hpp"
+
+#include <rj_geometry/util.hpp>
+
+#include "control/trapezoidal_motion.hpp"
+#include "escape_obstacles_path_planner.hpp"
+#include "planning/primitives/create_path.hpp"
+#include "planning/trajectory_utils.hpp"
+
+using namespace std;
+using namespace rj_geometry;
+
+namespace planning {
+
+Trajectory LineKickPlannerTwo::plan(const PlanRequest& plan_request) {
+    // If we are not allowed to touch the ball, this planner always fails
+    // This is preferred to simply ending the planner because it is possible (likely)
+    // that strategy re-requests the same planner anyway.
+    if (plan_request.play_state == PlayState::halt() ||
+        plan_request.play_state == PlayState::stop()) {
+        return Trajectory{};
+    }
+
+    const BallState& ball = plan_request.world_state->ball;
+
+    if (!average_ball_vel_initialized_) {
+        average_ball_vel_ = ball.velocity;
+        average_ball_vel_initialized_ = true;
+    } else {
+        // Add the newest ball velocity measurement to the average velocity
+        // estimate, but downweight the new value heavily
+        //
+        // e.g. new_avg_vel = (0.8 * avg_vel) + (0.2 * new_vel)
+        //
+        average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8);
+    }
+
+    prev_path_ = final(plan_request);
+
+    prev_path_.stamp(RJ::now());
+    return prev_path_;
+}
+
+Trajectory LineKickPlannerTwo::final(const PlanRequest& plan_request) {
+    const BallState& ball = plan_request.world_state->ball;
+
+    // Velocity is the speed (parameter) times the unit vector in the correct direction
+    auto goal_to_ball = (plan_request.motion_command.target.position - ball.position);
+    auto vel = goal_to_ball.normalized() * kFinalRobotSpeed;
+
+    // Create an updated MotionCommand and forward to PathTargetPathPlaner
+    PlanRequest modified_request = plan_request;
+
+    LinearMotionInstant target{ball.position, vel};
+
+    MotionCommand modified_command{"path_target", target,
+                                   FacePoint{plan_request.motion_command.target.position}};
+    modified_request.motion_command = modified_command;
+
+    return path_target_.plan(modified_request);
+}
+
+bool LineKickPlannerTwo::is_done() const {
+    // if ball is fast, assume we have kicked it correctly
+    // (either way we can't go recapture it)
+    return average_ball_vel_.mag() > kIsDoneBallVel;
+}
+}  // namespace planning

soccer/src/soccer/planning/planner/line_kick_planner_two.hpp

@@ -0,0 +1,63 @@
+#pragma once
+
+#include <optional>
+
+#include "planning/planner/path_planner.hpp"
+#include "planning/planner/path_target_path_planner.hpp"
+#include "planning/trajectory.hpp"
+
+namespace planning {
+
+/**
+ * PathPlanner which plans a path to line kick a ball.
+ * It takes the ball's current trajectory and plans a path for the robot
+ * to intersect with the ball.
+ *
+ * It is in two stages. The first stage drives up to the ball, trying to avoid
+ * accidentally tapping it out of place. The second stage drives directly
+ * into the ball, hopefully kicking it if the hardware has been instructed
+ * to kick.
+ *
+ * Because of the two-stage system, this planner is *stateful*. It ought
+ * not to be destructed and re-constructed during a single execution;
+ * the best approach is to call plan() on each tick.
+ * However, it also shouldn't *completely* break if it is reset.
+ *
+ * Params taken from MotionCommand:
+ *   target.position - planner will kick to this point
+ */
+class LineKickPlannerTwo : public PathPlanner {
+public:
+    LineKickPlannerTwo() : PathPlanner("line_kick_two"){};
+    Trajectory plan(const PlanRequest& plan_request) override;
+
+    void reset() override {
+        prev_path_ = {};
+        average_ball_vel_initialized_ = false;
+    }
+
+    [[nodiscard]] bool is_done() const override;
+
+private:
+    PathTargetPathPlanner path_target_{};
+    Trajectory prev_path_;
+
+    // These constants could be tuned more
+    static constexpr double kIsDoneBallVel{1.5};
+    static constexpr double kFinalRobotSpeed{1.0};
+    static constexpr double kPredictIn{0.5};  // seconds
+    static constexpr double kAvoidBallBy{0.05};
+    static constexpr double kLowPassFilterGain{0.2};
+
+    rj_geometry::Point average_ball_vel_;
+    bool average_ball_vel_initialized_ = false;
+
+    /**
+     * Returns the trajectory during the final stage.
+     * Uses PathTargetPathPlanner to draw a path directly into the ball.
+     * Tries to hit the ball with the mouth of the robot.
+     */
+    Trajectory final(const PlanRequest& plan_request);
+};
+
+}  // namespace planning

soccer/src/soccer/planning/planner_for_robot.cpp

@@ -6,6 +6,8 @@
 #include "planning/planner/goalie_idle_path_planner.hpp"
 #include "planning/planner/intercept_path_planner.hpp"
 #include "planning/planner/line_kick_path_planner.hpp"
+#include "planning/planner/line_kick_planner_one.hpp"
+#include "planning/planner/line_kick_planner_two.hpp"
 #include "planning/planner/path_target_path_planner.hpp"
 #include "planning/planner/pivot_path_planner.hpp"
 #include "planning/planner/settle_path_planner.hpp"
@@ -29,6 +31,8 @@ PlannerForRobot::PlannerForRobot(int robot_id, rclcpp::Node* node,
     path_planners_[SettlePathPlanner().name()] = std::make_unique<SettlePathPlanner>();
     path_planners_[CollectPathPlanner().name()] = std::make_unique<CollectPathPlanner>();
     path_planners_[LineKickPathPlanner().name()] = std::make_unique<LineKickPathPlanner>();
+    path_planners_[LineKickPlannerOne().name()] = std::make_unique<LineKickPlannerOne>();
+    path_planners_[LineKickPlannerTwo().name()] = std::make_unique<LineKickPlannerTwo>();
     path_planners_[PivotPathPlanner().name()] = std::make_unique<PivotPathPlanner>();
     path_planners_[EscapeObstaclesPathPlanner().name()] =
         std::make_unique<EscapeObstaclesPathPlanner>();