From 26caeb4c4612e10f4f6c9ed12438e731aed7bfc7 Mon Sep 17 00:00:00 2001 From: Peter Garud <79226125+petergarud@users.noreply.github.com> Date: Fri, 26 Jan 2024 20:02:13 -0500 Subject: [PATCH] Rewrite linekicker (#2163) Co-authored-by: Sid Parikh Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: petergarud Co-authored-by: sid-parikh --- .../include/rj_constants/constants.hpp | 3 + .../planner/line_kick_path_planner.cpp | 279 ++++-------------- .../planner/line_kick_path_planner.hpp | 35 ++- .../soccer/planning/planner/plan_request.cpp | 5 +- .../soccer/planning/planner/plan_request.hpp | 1 + .../strategy/agent/position/offense.cpp | 18 +- 6 files changed, 105 insertions(+), 236 deletions(-) diff --git a/rj_constants/include/rj_constants/constants.hpp b/rj_constants/include/rj_constants/constants.hpp index fb83fd3bde0..199101e80c1 100644 --- a/rj_constants/include/rj_constants/constants.hpp +++ b/rj_constants/include/rj_constants/constants.hpp @@ -30,5 +30,8 @@ constexpr float kDotsSmallOffset = 0.035; constexpr float kDotsLargeOffset = 0.054772; constexpr float kDotsRadius = 0.02; +/** constants for planning */ +constexpr double kAvoidBallDistance = 0.01; + const std::string kTeamNameLower = "robojackets"; const std::string kTeamName = "RoboJackets"; diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp index 773b30b1917..f12219ffcf4 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -1,4 +1,4 @@ -#include "line_kick_path_planner.hpp" +#include "planning/planner/line_kick_path_planner.hpp" #include @@ -13,30 +13,13 @@ using namespace rj_geometry; namespace planning { Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { - // TODO(?): ros param these - const float approach_speed = 0.05; - - const float ball_avoid_distance = 0.10; - - const MotionCommand& command = plan_request.motion_command; - - if (plan_request.virtual_obstacles.hit(plan_request.start.position())) { - prev_path_ = Trajectory{}; - return prev_path_; + if (plan_request.play_state == PlayState::halt() || + plan_request.play_state == PlayState::stop()) { + return Trajectory{}; } - if (target_kick_pos_.has_value() && - command.target.position.dist_to(target_kick_pos_.value()) > 0.1) { - prev_path_ = Trajectory{}; - target_kick_pos_ = std::nullopt; - } + const BallState& ball = plan_request.world_state->ball; - const RobotInstant& start_instant = plan_request.start; - const auto& motion_constraints = plan_request.constraints.mot; - const auto& rotation_constraints = plan_request.constraints.rot; - const auto& ball = plan_request.world_state->ball; - - // track ball velocity to know if done or not if (!average_ball_vel_initialized_) { average_ball_vel_ = ball.velocity; average_ball_vel_initialized_ = true; @@ -50,230 +33,84 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8); } - if (prev_path_.empty()) { - final_approach_ = false; + // only process state transition if already started the planning + if (has_created_plan) { + process_state_transition(); } - - ShapeSet static_obstacles; - std::vector dynamic_obstacles; - fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false, nullptr); - - auto obstacles_with_ball = static_obstacles; - const RJ::Time cur_time = start_instant.stamp; - obstacles_with_ball.add( - make_shared(ball.predict_at(cur_time).position, ball_avoid_distance)); - - // This segfaults. Please rewrite the entire thing. -#if 0 - if (final_approach_ && target_kick_pos_) { - RJ::Seconds duration_into_path = cur_time - prev_path_.begin_time(); - - RobotInstant target = prev_path_.last(); - RJ::Time time = ball.query_time_near(*target_kick_pos_); - - auto time_left = prev_path_.duration() - duration_into_path; - - if (time_left < RJ::Seconds(-0.3) || time_left > RJ::Seconds(5.0)) { - final_approach_ = false; - prev_path_ = {}; - } else if (time_left < RJ::Seconds(0)) { - prev_path_.set_debug_text("reuse past done " + std::to_string(time_left.count())); - return prev_path_; - } else { - RJ::Seconds time_for_ball = time - cur_time; - prev_path_.scale_duration(prev_path_.duration() * (time_left / time_for_ball), - start_instant.stamp); - prev_path_.set_debug_text("reuse final slow " + std::to_string(time_for_ball.count()) + - " " + std::to_string(time_left.count())); - prev_path_.stamp(RJ::now()); - return prev_path_; - } + switch (current_state_) { + case INITIAL_APPROACH: + prev_path_ = initial(plan_request); + break; + case FINAL_APPROACH: + prev_path_ = final(plan_request); + break; } -#endif - - // only plan line kick if not is_done - if (!this->is_done()) { - LinearMotionInstant target{ - ball.position, (command.target.position - ball.position).normalized(approach_speed)}; - - auto ball_trajectory = ball.make_trajectory(); - - Trajectory path; - if (std::abs(target.velocity.angle_between((target.position - start_instant.position()))) > - degrees_to_radians(10)) { - target.position -= target.velocity.normalized(ball_avoid_distance * 2 + kRobotRadius); - if (!prev_path_.empty() && - target.position.dist_to(prev_path_.last().position()) < - Replanner::goal_pos_change_threshold() && - reuse_path_count_ < 20) { - target.velocity = prev_path_.last().linear_velocity(); - reuse_path_count_++; - } else { - reuse_path_count_ = 0; - } + prev_path_.stamp(RJ::now()); + has_created_plan = true; + return prev_path_; +} - Replanner::PlanParams params{ - start_instant, - target, - obstacles_with_ball, - dynamic_obstacles, - plan_request.constraints, - AngleFns::face_angle(ball.position.angle_to(command.target.position))}; - path = Replanner::create_plan(params, prev_path_); - path.set_debug_text("slow ball 1"); - } else { - if (!prev_path_.empty() && - target.position.dist_to(prev_path_.last().position()) < - Replanner::goal_pos_change_threshold() && - reuse_path_count_ < 20) { - target.velocity = prev_path_.last().linear_velocity(); - reuse_path_count_++; - } else { - reuse_path_count_ = 0; - } +Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { + // Getting ball info + const BallState& ball = plan_request.world_state->ball; - target.position += target.velocity.normalized(kRobotRadius); + // Creating a modified plan_request to send to PathTargetPlanner + PlanRequest modified_request = plan_request; - Replanner::PlanParams params{start_instant, - target, - static_obstacles, - dynamic_obstacles, - plan_request.constraints, - AngleFns::face_point(command.target.position)}; - path = Replanner::create_plan(params, prev_path_); - path.set_debug_text("slow ball 2"); - } - target_kick_pos_ = command.target.position; - path.stamp(RJ::now()); - prev_path_ = path; - return path; - } + // Where to navigate to + auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy; - return Trajectory{}; - // (Kevin) pretty sure everything under this line is not used + auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position; - if (!prev_path_.empty() && target_kick_pos_) { - auto previous_duration_remaining = prev_path_.end_time() - start_instant.stamp; + auto goal_to_ball = (plan_request.motion_command.target.position - ball_position); + auto offset_from_ball = distance_from_ball * goal_to_ball.normalized(); - LinearMotionInstant target; - RJ::Time intercept_time = ball.query_time_near(*target_kick_pos_, &target.position); - RJ::Time end_time_adjusted = prev_path_.end_time() - RJ::Seconds(1.0); - if (previous_duration_remaining < RJ::Seconds(0.0)) { - target.velocity = - (command.target.position - target.position).normalized(approach_speed); - target.position -= target.velocity.normalized(kRobotRadius + kBallRadius * 2); + // Updating the motion command + LinearMotionInstant target{ball_position - offset_from_ball}; - Replanner::PlanParams params{start_instant, - target, - static_obstacles, - dynamic_obstacles, - plan_request.constraints, - AngleFns::face_point(command.target.position)}; - Trajectory path = Replanner::create_plan(params, prev_path_); + MotionCommand modified_command{"path_target", target, + FacePoint{plan_request.motion_command.target.position}}; + modified_request.motion_command = modified_command; - if (!path.empty()) { - path.set_debug_text( - "FinalPath" + std::to_string(path.duration().count()) + " " + - std::to_string(RJ::Seconds(intercept_time - start_instant.stamp).count()) + - " " + std::to_string(intercept_time.time_since_epoch().count())); - path.stamp(RJ::now()); - prev_path_ = path; - return path; - } - } + // Getting the new path from PathTargetPlanner + Trajectory path = path_target_.plan(modified_request); - if (prev_path_.check_time(start_instant.stamp) && - !trajectory_hits_static(prev_path_, static_obstacles, start_instant.stamp, nullptr) && - end_time_adjusted < intercept_time && reuse_path_count_ < 10) { - reuse_path_count_++; - Point near_point; - prev_path_.set_debug_text("Reuse prev_path"); - if (ball.query_time_near(prev_path_.last().position(), &near_point) >= - end_time_adjusted) { - return prev_path_; - } - } - } + return path; +} - Trajectory partial_path; - RJ::Seconds partial_path_time = 0ms; - auto tmp_start_instant = start_instant; - const auto partial_replan_lead_time = RJ::Seconds(Replanner::partial_replan_lead_time()); +Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { + // remove ball from obstacles not needed? - if (!prev_path_.empty() && prev_path_.check_time(start_instant.stamp)) { - if (start_instant.stamp < prev_path_.end_time() - partial_replan_lead_time * 2) { - partial_path = prev_path_.sub_trajectory( - start_instant.stamp, start_instant.stamp + partial_replan_lead_time); - partial_path_time = partial_replan_lead_time; - tmp_start_instant = partial_path.last(); - } - } + const BallState& ball = plan_request.world_state->ball; - for (auto t = RJ::Seconds(0.4); t < RJ::Seconds(6); t += RJ::Seconds(0.2)) { - RJ::Time rollout_time = cur_time + t; + // Creating a modified plan_request to send to PathTargetPlanner + PlanRequest modified_request = plan_request; - auto ball_state_predicted = ball.predict_at(rollout_time); - LinearMotionInstant target{ball_state_predicted.position}; - target_kick_pos_ = target.position; - target.velocity = (command.target.position - target.position).normalized(approach_speed); - target.position -= target.velocity.normalized(kRobotRadius + kBallRadius * 2); + // velocity + auto goal_to_ball = (plan_request.motion_command.target.position - ball.position); + auto vel = goal_to_ball.normalized() * kFinalRobotSpeed; - vector intermediate_points; - if (std::abs(target.velocity.angle_between( - (target.position - tmp_start_instant.position()))) > degrees_to_radians(60)) { - intermediate_points.push_back( - target.position - - target.velocity.normalized(kRobotRadius * 2.0 + kBallRadius * 2.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; - Trajectory path = CreatePath::simple(tmp_start_instant.linear_motion(), target, - plan_request.constraints.mot, tmp_start_instant.stamp, - intermediate_points); + // Getting the new path from PathTargetPlanner + Trajectory path = path_target_.plan(modified_request); - if (!path.empty()) { - if (path.duration() + partial_path_time <= t) { - if (!partial_path.empty()) { - path = Trajectory(std::move(partial_path), path); - } - plan_angles(&path, tmp_start_instant, AngleFns::face_point(target.position), - plan_request.constraints.rot); + return path; +} - path.set_debug_text("FoundPath" + std::to_string(path.duration().count())); - reuse_path_count_ = 0; - path.stamp(RJ::now()); - prev_path_ = path; - return path; - } - } +void LineKickPathPlanner::process_state_transition() { + if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) { + current_state_ = FINAL_APPROACH; } - - auto ball_predicted = ball.predict_at(cur_time); - LinearMotionInstant target{ball_predicted.position}; - target.velocity = (command.target.position - target.position).normalized(approach_speed); - target.position -= target.velocity.normalized(kRobotRadius * 3); - - auto ball_path = ball.make_trajectory(); - dynamic_obstacles.emplace_back(kBallRadius, &ball_path); - - Replanner::PlanParams params{start_instant, - target, - static_obstacles, - dynamic_obstacles, - plan_request.constraints, - AngleFns::face_point(command.target.position)}; - Trajectory path = Replanner::create_plan(params, prev_path_); - - path.set_debug_text("Approaching cautious"); - - path.stamp(RJ::now()); - prev_path_ = path; - return path; } bool LineKickPathPlanner::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() > IS_DONE_BALL_VEL; + return average_ball_vel_.mag() > kIsDoneBallVel; } } // namespace planning diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp index e7f693110f8..0e451c79764 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -2,7 +2,9 @@ #include +#include "planning/planner/collect_path_planner.hpp" #include "planning/planner/path_planner.hpp" +#include "planning/planner/path_target_path_planner.hpp" #include "planning/trajectory.hpp" class Configuration; @@ -29,25 +31,40 @@ class LineKickPathPlanner : public PathPlanner { void reset() override { prev_path_ = {}; - final_approach_ = false; target_kick_pos_ = std::nullopt; - reuse_path_count_ = 0; + current_state_ = INITIAL_APPROACH; + average_ball_vel_initialized_ = false; + has_created_plan = false; } [[nodiscard]] bool is_done() const override; private: + enum State { INITIAL_APPROACH, FINAL_APPROACH }; + State current_state_ = INITIAL_APPROACH; + PathTargetPathPlanner path_target_{}; + CollectPathPlanner collect_planner_{}; Trajectory prev_path_; - bool final_approach_ = false; - std::optional target_kick_pos_; - int reuse_path_count_ = 0; - // ball velocity filtering vars + // 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; + rj_geometry::Point average_ball_vel_; bool average_ball_vel_initialized_ = false; + bool has_created_plan = false; + std::optional target_kick_pos_; + + // Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant, + // ShapeSet static_obstacles, std::vector dynamic_obstacles); + Trajectory initial(const PlanRequest& plan_request); + Trajectory final(const PlanRequest& plan_request); + // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet + // static_obstacles, std::vector dynamic_obstacles); + void process_state_transition(); - // TODO(Kevin): make this a common param ("ball is slow" used - // in a lot of places) - double IS_DONE_BALL_VEL = 0.5; // m/s + // PlayState::State current_state_; }; } // namespace planning diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index abb01fd8126..09a7c225ebe 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -66,8 +66,9 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, // Adding ball as a static obstacle (because dynamic obstacles are not working) // Only added when STOP state is enabled if (in.min_dist_from_ball > 0 || avoid_ball) { - auto ball_obs = make_inflated_static_obs(in.world_state->ball.position, - in.world_state->ball.velocity, kBallRadius); + auto ball_obs = + make_inflated_static_obs(in.world_state->ball.position, in.world_state->ball.velocity, + kBallRadius + kAvoidBallDistance); ball_obs.radius(ball_obs.radius() + in.min_dist_from_ball); // Draw ball obstacle in simulator diff --git a/soccer/src/soccer/planning/planner/plan_request.hpp b/soccer/src/soccer/planning/planner/plan_request.hpp index a96f207ed5f..54eb9a0ffa5 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -110,6 +110,7 @@ struct PlanRequest { // Whether the robot has a ball bool ball_sense = false; + /** /** * How far away to stay from the ball, if the MotionCommand chooses to avoid the ball. */ diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 49e8bea70bd..689e50eda7d 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -29,8 +29,8 @@ Offense::State Offense::update_state() { double distance_to_ball = robot_position.dist_to(ball_position); if (current_state_ == IDLING) { - // send_scorer_request(); - next_state = AWAITING_SEND_PASS; + send_scorer_request(); + next_state = SHOOTING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -83,10 +83,12 @@ Offense::State Offense::update_state() { } } - return next_state; + return SHOOTING; } std::optional Offense::state_to_task(RobotIntent intent) { + SPDLOG_INFO(current_state_); + if (current_state_ == IDLING) { // Do nothing auto empty_motion_cmd = planning::MotionCommand{}; @@ -98,6 +100,7 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == PASSING) { + target_robot_id = 2; rj_geometry::Point target_robot_pos = last_world_state_->get_robot(true, target_robot_id).pose.position(); planning::LinearMotionInstant target{target_robot_pos}; @@ -126,13 +129,20 @@ std::optional 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() * 3.0 / 8.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; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; intent.kick_speed = 4.0; intent.is_active = true; + + // intent.motion_command = planning::MotionCommand{ + // "path_target", planning::LinearMotionInstant{last_world_state_->ball.position, + // {0.0}}, planning::FaceBall{}}; + return intent; } else if (current_state_ == RECEIVING) { // check how far we are from the ball