From bf547aad9fbe26780ffe53ceddb55a0f00124f83 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 20 Nov 2023 02:05:54 +0000 Subject: [PATCH 01/15] added play state to plan request --- soccer/src/soccer/planning/planner/plan_request.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/soccer/src/soccer/planning/planner/plan_request.hpp b/soccer/src/soccer/planning/planner/plan_request.hpp index a96f207ed5f..5bcab63892f 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -10,6 +10,7 @@ #include "../global_state.hpp" #include "context.hpp" +#include "../global_state.hpp" #include "planning/dynamic_obstacle.hpp" #include "planning/instant.hpp" #include "planning/robot_constraints.hpp" @@ -29,9 +30,9 @@ struct PlanRequest { PlanRequest(RobotInstant start, MotionCommand command, // NOLINT RobotConstraints constraints, rj_geometry::ShapeSet field_obstacles, rj_geometry::ShapeSet virtual_obstacles, TrajectoryCollection* planned_trajectories, - unsigned shell_id, const WorldState* world_state, PlayState play_state, - int8_t priority = 0, rj_drawing::RosDebugDrawer* debug_drawer = nullptr, - bool ball_sense = false, float min_dist_from_ball = 0, float dribbler_speed = 0) + unsigned shell_id, const WorldState* world_state, PlayState play_state, int8_t priority = 0, + rj_drawing::RosDebugDrawer* debug_drawer = nullptr, bool ball_sense = false, + float min_dist_from_ball = 0, float dribbler_speed = 0) : start(start), motion_command(command), // NOLINT constraints(constraints), @@ -41,6 +42,7 @@ struct PlanRequest { shell_id(shell_id), priority(priority), world_state(world_state), + play_state_(play_state), play_state(play_state), debug_drawer(debug_drawer), ball_sense(ball_sense), @@ -119,6 +121,7 @@ struct PlanRequest { * Dribbler Speed */ float dribbler_speed = 0; + }; /** From ade7c78393123789056a7114efe653fd90e4c0cb Mon Sep 17 00:00:00 2001 From: petergarud Date: Wed, 10 Jan 2024 02:06:51 +0000 Subject: [PATCH 02/15] add all to linekicker --- .../planning/planner/line_kick_path_planner.cpp | 17 ++++++++++++++--- .../soccer/strategy/agent/position/offense.cpp | 5 +++-- 2 files changed, 17 insertions(+), 5 deletions(-) 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..ecf61b4a8b3 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -13,6 +13,11 @@ using namespace rj_geometry; namespace planning { Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { + + if (plan_request.play_state_ == PlayState::halt() || plan_request.play_state_ == PlayState::stop()) { + return Trajectory{}; + } + // TODO(?): ros param these const float approach_speed = 0.05; @@ -64,7 +69,8 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { make_shared(ball.predict_at(cur_time).position, ball_avoid_distance)); // This segfaults. Please rewrite the entire thing. -#if 0 + // I'm not seeing a segfault +//#if 0 if (final_approach_ && target_kick_pos_) { RJ::Seconds duration_into_path = cur_time - prev_path_.begin_time(); @@ -89,7 +95,11 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { return prev_path_; } } -#endif +//#endif + + if (target_kick_pos_) { + SPDLOG_INFO("Point(" + to_string(target_kick_pos_.value().x()) + ", " + to_string(target_kick_pos_.value().y()) + ")"); + } // only plan line kick if not is_done if (!this->is_done()) { @@ -146,10 +156,11 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { target_kick_pos_ = command.target.position; path.stamp(RJ::now()); prev_path_ = path; + //SPDLOG_INFO(path); return path; } - return Trajectory{}; + //return Trajectory{}; // (Kevin) pretty sure everything under this line is not used if (!prev_path_.empty() && target_kick_pos_) { diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 49e8bea70bd..4aa35eaae52 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 = PASSING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -98,6 +98,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}; From 9b1671f3c82212f03492d81e89eff2b175db4995 Mon Sep 17 00:00:00 2001 From: petergarud Date: Wed, 10 Jan 2024 02:53:06 +0000 Subject: [PATCH 03/15] starting removal of linekicker --- .../planner/line_kick_path_planner.cpp | 20 ++++--------------- .../planner/line_kick_path_planner.hpp | 17 ++++++---------- 2 files changed, 10 insertions(+), 27 deletions(-) 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 ecf61b4a8b3..c8b50f5cb54 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -13,33 +13,21 @@ using namespace rj_geometry; namespace planning { Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { - if (plan_request.play_state_ == PlayState::halt() || plan_request.play_state_ == PlayState::stop()) { return Trajectory{}; } - // TODO(?): ros param these - const float approach_speed = 0.05; - - const float ball_avoid_distance = 0.10; - + BallState ball = plan_request.world_state->ball; const MotionCommand& command = plan_request.motion_command; + const RobotInstant& start_instant = plan_request.start; + const auto& motion_constraints = plan_request.constraints.mot; + const auto& rotation_constraints = plan_request.constraints.rot; if (plan_request.virtual_obstacles.hit(plan_request.start.position())) { prev_path_ = Trajectory{}; return prev_path_; } - 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 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_) { 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..90a03363a09 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -29,25 +29,20 @@ class LineKickPathPlanner : public PathPlanner { void reset() override { prev_path_ = {}; - final_approach_ = false; target_kick_pos_ = std::nullopt; - reuse_path_count_ = 0; + latest_state_ = IDLING; + average_ball_vel_initialized_ = false; } [[nodiscard]] bool is_done() const override; private: + State latest_state_ = IDLING; + enum State { + IDLING, INITIAL_APPROACH, FINAL_APPROACH + }; Trajectory prev_path_; - bool final_approach_ = false; std::optional target_kick_pos_; - int reuse_path_count_ = 0; - - // ball velocity filtering vars - rj_geometry::Point average_ball_vel_; bool average_ball_vel_initialized_ = false; - - // 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 }; } // namespace planning From 68fde4c6b428118c507781eebb753ecc5eda8500 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 15 Jan 2024 01:30:15 +0000 Subject: [PATCH 04/15] setup for linekicker logic --- .../planner/line_kick_path_planner.cpp | 234 ++---------------- .../planner/line_kick_path_planner.hpp | 9 +- 2 files changed, 29 insertions(+), 214 deletions(-) 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 c8b50f5cb54..f17e318adba 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -20,16 +20,7 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { BallState ball = plan_request.world_state->ball; const MotionCommand& command = plan_request.motion_command; const RobotInstant& start_instant = plan_request.start; - const auto& motion_constraints = plan_request.constraints.mot; - const auto& rotation_constraints = plan_request.constraints.rot; - if (plan_request.virtual_obstacles.hit(plan_request.start.position())) { - prev_path_ = Trajectory{}; - return prev_path_; - } - - - // track ball velocity to know if done or not if (!average_ball_vel_initialized_) { average_ball_vel_ = ball.velocity; average_ball_vel_initialized_ = true; @@ -43,10 +34,6 @@ 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; - } - ShapeSet static_obstacles; std::vector dynamic_obstacles; fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false, nullptr); @@ -56,217 +43,42 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { obstacles_with_ball.add( make_shared(ball.predict_at(cur_time).position, ball_avoid_distance)); - // This segfaults. Please rewrite the entire thing. - // I'm not seeing a segfault -//#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_; - } - } -//#endif - - if (target_kick_pos_) { - SPDLOG_INFO("Point(" + to_string(target_kick_pos_.value().x()) + ", " + to_string(target_kick_pos_.value().y()) + ")"); - } - // 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; - } - - 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; - } - target.position += target.velocity.normalized(kRobotRadius); + switch(current_state_) { + case INITIAL_APPROACH: + prev_path_ = initial(ball, command, start_instant, static_obstacles, dynamic_obstacles); + break; + case FINAL_APPROACH: + prev_path_ = final(ball, command, start_instant, static_obstacles, dynamic_obstacles); + break; - 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; - //SPDLOG_INFO(path); - return path; - } - - //return Trajectory{}; - // (Kevin) pretty sure everything under this line is not used - - if (!prev_path_.empty() && target_kick_pos_) { - auto previous_duration_remaining = prev_path_.end_time() - start_instant.stamp; - - 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); - - 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_); - - 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; - } - } - - 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_; - } - } - } - - 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()); - - 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(); + prev_path_.stamp(RJ::now()); + return prev_path_; } } + return prev_path; +} - for (auto t = RJ::Seconds(0.4); t < RJ::Seconds(6); t += RJ::Seconds(0.2)) { - RJ::Time rollout_time = cur_time + t; - - 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); +Trajectory LineKickPathPlanner::initial(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles) { + Trajectory path; + LinearMotionInstant target{ball.position}; - 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)); - } + auto ball_trajectory = ball.make_trajectory(); - Trajectory path = CreatePath::simple(tmp_start_instant.linear_motion(), target, - plan_request.constraints.mot, tmp_start_instant.stamp, - intermediate_points); + return path; +} - 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); +Trajectory LineKickPathPlanner::final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles) { + return prev_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::state_transition(BallState ball, RobotInstant start_instant) { + if (current_state_ == INITIAL_APPROACH && (ball.position - start_instant.position()).mag() < 0.1) { + 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 { 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 90a03363a09..70e50d40b05 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -30,19 +30,22 @@ class LineKickPathPlanner : public PathPlanner { void reset() override { prev_path_ = {}; target_kick_pos_ = std::nullopt; - latest_state_ = IDLING; + current_state_ = INITIAL_APPROACH; average_ball_vel_initialized_ = false; } [[nodiscard]] bool is_done() const override; private: - State latest_state_ = IDLING; + State current_state_ = IDLING; enum State { - IDLING, INITIAL_APPROACH, FINAL_APPROACH + INITIAL_APPROACH, FINAL_APPROACH }; Trajectory prev_path_; std::optional target_kick_pos_; bool average_ball_vel_initialized_ = false; + Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles); + Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles); + void state_transition(BallState ball, RobotInstant start_instant); }; } // namespace planning From 5184c0987f8abe1c091f5156d5b6b7e713b2a986 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 15 Jan 2024 02:39:13 +0000 Subject: [PATCH 05/15] using settle for initial approach --- .../planner/line_kick_path_planner.cpp | 44 ++++++++----------- .../planner/line_kick_path_planner.hpp | 12 +++-- 2 files changed, 28 insertions(+), 28 deletions(-) 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 f17e318adba..a6329acaf09 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 @@ -17,9 +17,9 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { return Trajectory{}; } - BallState ball = plan_request.world_state->ball; - const MotionCommand& command = plan_request.motion_command; - const RobotInstant& start_instant = plan_request.start; + const BallState& ball = plan_request.world_state->ball; + // const MotionCommand& command = plan_request.motion_command; + // const RobotInstant& start_instant = plan_request.start; if (!average_ball_vel_initialized_) { average_ball_vel_ = ball.velocity; @@ -34,24 +34,24 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8); } - ShapeSet static_obstacles; - std::vector dynamic_obstacles; - fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false, nullptr); + // 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)); + // 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)); - // only plan line kick if not is_done + // // only plan line kick if not is_done if (!this->is_done()) { - + state_transition(ball, plan_request.start); switch(current_state_) { case INITIAL_APPROACH: - prev_path_ = initial(ball, command, start_instant, static_obstacles, dynamic_obstacles); + prev_path_ = initial(plan_request); break; case FINAL_APPROACH: - prev_path_ = final(ball, command, start_instant, static_obstacles, dynamic_obstacles); + prev_path_ = final(plan_request); break; } @@ -59,20 +59,14 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { return prev_path_; } } - return prev_path; -} - -Trajectory LineKickPathPlanner::initial(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles) { - Trajectory path; - LinearMotionInstant target{ball.position}; - - auto ball_trajectory = ball.make_trajectory(); +Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { + Trajectory path = settle_.plan(plan_request); return path; } -Trajectory LineKickPathPlanner::final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles) { - return prev_path; +Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { + return prev_path_; } void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_instant) { 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 70e50d40b05..665e872ba34 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -4,6 +4,7 @@ #include "planning/planner/path_planner.hpp" #include "planning/trajectory.hpp" +#include "planning/planner/settle_path_planner.hpp" class Configuration; class ConfigDouble; @@ -36,15 +37,20 @@ class LineKickPathPlanner : public PathPlanner { [[nodiscard]] bool is_done() const override; private: - State current_state_ = IDLING; enum State { INITIAL_APPROACH, FINAL_APPROACH }; + State current_state_ = INITIAL_APPROACH; + SettlePathPlanner settle_{}; Trajectory prev_path_; + static constexpr double IS_DONE_BALL_VEL = 0.5; + rj_geometry::Point average_ball_vel_; std::optional target_kick_pos_; bool average_ball_vel_initialized_ = false; - Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles); - Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles); + // 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 state_transition(BallState ball, RobotInstant start_instant); }; From be204483009ddecc4b4e32b6477c074f3904f981 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 15 Jan 2024 02:53:38 +0000 Subject: [PATCH 06/15] with path to target --- .../soccer/planning/planner/line_kick_path_planner.cpp | 8 +++++++- .../soccer/planning/planner/line_kick_path_planner.hpp | 4 ++-- 2 files changed, 9 insertions(+), 3 deletions(-) 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 a6329acaf09..3ae4db62d98 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -61,7 +61,13 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { } Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { - Trajectory path = settle_.plan(plan_request); + const BallState& ball = plan_request.world_state->ball; + PlanRequest modified_request = plan_request; + Point target_pos = ball.position; + LinearMotionInstant target{target_pos, {0, 0}}; + MotionCommand modified_command{"line_kick", target}; + modified_request.motion_command = modified_command; + Trajectory path = path_target_.plan(modified_request); return path; } 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 665e872ba34..1af7b49cb4b 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -4,7 +4,7 @@ #include "planning/planner/path_planner.hpp" #include "planning/trajectory.hpp" -#include "planning/planner/settle_path_planner.hpp" +#include "planning/planner/path_target_path_planner.hpp" class Configuration; class ConfigDouble; @@ -41,7 +41,7 @@ class LineKickPathPlanner : public PathPlanner { INITIAL_APPROACH, FINAL_APPROACH }; State current_state_ = INITIAL_APPROACH; - SettlePathPlanner settle_{}; + PathTargetPathPlanner path_target_{}; Trajectory prev_path_; static constexpr double IS_DONE_BALL_VEL = 0.5; rj_geometry::Point average_ball_vel_; From 59668615a6605ffe21e05914465282ab7688caa4 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 15 Jan 2024 18:27:09 +0000 Subject: [PATCH 07/15] only recalculates plan when ball moved certain distance --- .../planner/line_kick_path_planner.cpp | 32 +++++++++++++++++-- .../planner/line_kick_path_planner.hpp | 4 ++- 2 files changed, 33 insertions(+), 3 deletions(-) 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 3ae4db62d98..8fc79e44caf 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -61,12 +61,40 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { } Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { + // Getting ball info const BallState& ball = plan_request.world_state->ball; + + // Creating a modified plan_request to send to PathTargetPlanner PlanRequest modified_request = plan_request; - Point target_pos = ball.position; - LinearMotionInstant target{target_pos, {0, 0}}; + + // Determining where to navigate to + // Goes to where ball is predicted to be in 0.1 seconds + // If this prediction is within .1 meters of initial prediction, then the plan is not recalculated + const RJ::Seconds predictTime(0.1); + const RJ::Time cur_time = plan_request.start.stamp + predictTime; + Point ball_prediction = ball.predict_at(cur_time).position; + if (target_kick_pos_.has_value()) { + double distanceBetween = (target_kick_pos_.value() - ball_prediction).mag(); + if (distanceBetween > 0.1) { + target_kick_pos_ = ball_prediction; + } else { + return prev_path_; + } + } else { + target_kick_pos_ = ball_prediction; + } + + // Determining the velocity + Point approach_direction = ball_prediction - plan_request.start.position(); + double approach_speed = 50.0; + Point target_vel = (average_ball_vel_ + approach_direction).normalized() * approach_speed; + + // Updating the motion command + LinearMotionInstant target{target_kick_pos_.value(), target_vel}; MotionCommand modified_command{"line_kick", target}; modified_request.motion_command = modified_command; + + // Getting the new path from PathTargetPlanner Trajectory path = path_target_.plan(modified_request); return path; } 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 1af7b49cb4b..10af3e45a78 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -43,10 +43,12 @@ class LineKickPathPlanner : public PathPlanner { State current_state_ = INITIAL_APPROACH; PathTargetPathPlanner path_target_{}; Trajectory prev_path_; + static constexpr double IS_DONE_BALL_VEL = 0.5; rj_geometry::Point average_ball_vel_; - std::optional target_kick_pos_; bool average_ball_vel_initialized_ = 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); From dea825f91f1ef8eade19121bde4198bdcbf9d0fa Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Tue, 16 Jan 2024 22:01:58 -0500 Subject: [PATCH 08/15] initial approach works --- .../include/rj_constants/constants.hpp | 3 + .../planner/line_kick_path_planner.cpp | 55 +++++++++---------- .../planner/line_kick_path_planner.hpp | 3 + .../soccer/planning/planner/plan_request.cpp | 2 +- .../strategy/agent/position/offense.cpp | 12 +++- 5 files changed, 42 insertions(+), 33 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 8fc79e44caf..68a80a0464e 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -13,7 +13,10 @@ using namespace rj_geometry; namespace planning { Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { - if (plan_request.play_state_ == PlayState::halt() || plan_request.play_state_ == PlayState::stop()) { + SPDLOG_INFO("Robot {} state {}", plan_request.shell_id, current_state_); + + if (plan_request.play_state_ == PlayState::halt() || + plan_request.play_state_ == PlayState::stop()) { return Trajectory{}; } @@ -46,65 +49,57 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { // // only plan line kick if not is_done if (!this->is_done()) { state_transition(ball, plan_request.start); - switch(current_state_) { + switch (current_state_) { case INITIAL_APPROACH: prev_path_ = initial(plan_request); break; case FINAL_APPROACH: prev_path_ = final(plan_request); break; - } prev_path_.stamp(RJ::now()); return prev_path_; - } } +} Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { - // Getting ball info + // // Getting ball info const BallState& ball = plan_request.world_state->ball; - // Creating a modified plan_request to send to PathTargetPlanner + // // Creating a modified plan_request to send to PathTargetPlanner PlanRequest modified_request = plan_request; - // Determining where to navigate to - // Goes to where ball is predicted to be in 0.1 seconds - // If this prediction is within .1 meters of initial prediction, then the plan is not recalculated - const RJ::Seconds predictTime(0.1); - const RJ::Time cur_time = plan_request.start.stamp + predictTime; - Point ball_prediction = ball.predict_at(cur_time).position; - if (target_kick_pos_.has_value()) { - double distanceBetween = (target_kick_pos_.value() - ball_prediction).mag(); - if (distanceBetween > 0.1) { - target_kick_pos_ = ball_prediction; - } else { - return prev_path_; - } - } else { - target_kick_pos_ = ball_prediction; - } + // const WorldState* world_state = plan_request.world_state; + + // Where to navigate to + auto distance_from_ball = kBallRadius + kRobotRadius + 0.05; - // Determining the velocity - Point approach_direction = ball_prediction - plan_request.start.position(); - double approach_speed = 50.0; - Point target_vel = (average_ball_vel_ + approach_direction).normalized() * approach_speed; + 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{target_kick_pos_.value(), target_vel}; - MotionCommand modified_command{"line_kick", target}; + LinearMotionInstant target{ball.position - offset_from_ball}; + + MotionCommand modified_command{"path_target", target, FaceBall{}}; 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) { - return prev_path_; + + + } void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_instant) { - if (current_state_ == INITIAL_APPROACH && (ball.position - start_instant.position()).mag() < 0.1) { + if (current_state_ == INITIAL_APPROACH && + (ball.position - start_instant.position()).mag() < 0.01) { current_state_ = FINAL_APPROACH; } } 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 10af3e45a78..3a6acf0073d 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -5,6 +5,7 @@ #include "planning/planner/path_planner.hpp" #include "planning/trajectory.hpp" #include "planning/planner/path_target_path_planner.hpp" +#include "planning/planner/collect_path_planner.hpp" class Configuration; class ConfigDouble; @@ -42,6 +43,7 @@ class LineKickPathPlanner : public PathPlanner { }; State current_state_ = INITIAL_APPROACH; PathTargetPathPlanner path_target_{}; + CollectPathPlanner collect_planner_{}; Trajectory prev_path_; static constexpr double IS_DONE_BALL_VEL = 0.5; @@ -54,6 +56,7 @@ class LineKickPathPlanner : public PathPlanner { Trajectory final(const PlanRequest& plan_request); // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles); void state_transition(BallState ball, RobotInstant start_instant); + }; } // namespace planning diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index abb01fd8126..d838b3b038e 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -67,7 +67,7 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, // 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); + 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/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 4aa35eaae52..342f648ce38 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -30,7 +30,7 @@ Offense::State Offense::update_state() { if (current_state_ == IDLING) { send_scorer_request(); - next_state = PASSING; + next_state = SHOOTING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -83,10 +83,13 @@ 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{}; @@ -134,6 +137,11 @@ std::optional Offense::state_to_task(RobotIntent intent) { 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 From 4514e2dfdb5c6868e02dda6eb8caf2cd68cb3699 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 21 Jan 2024 20:47:03 -0500 Subject: [PATCH 09/15] final appraoch plus kick works --- .../planner/line_kick_path_planner.cpp | 32 +++++++++++++------ .../planner/line_kick_path_planner.hpp | 3 +- .../strategy/agent/position/offense.cpp | 3 +- 3 files changed, 27 insertions(+), 11 deletions(-) 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 68a80a0464e..7459f0ebafe 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/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; } } 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 3a6acf0073d..4349e43c9f1 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/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 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 dynamic_obstacles); void state_transition(BallState ball, RobotInstant start_instant); + // PlayState::State current_state_; }; } // namespace planning diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 342f648ce38..8d27472e089 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -130,7 +130,8 @@ 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() / 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; From d263cf8f0c98739d24ca938bf51bfc9b7124b1cc Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 21 Jan 2024 20:51:53 -0500 Subject: [PATCH 10/15] Fix Code Style On rewrite-linekicker (#2164) automated style fixes Co-authored-by: petergarud --- .../planning/planner/line_kick_path_planner.cpp | 13 +++++++------ .../planning/planner/line_kick_path_planner.hpp | 14 +++++++------- .../src/soccer/planning/planner/plan_request.cpp | 5 +++-- .../src/soccer/planning/planner/plan_request.hpp | 9 ++++----- .../src/soccer/strategy/agent/position/offense.cpp | 5 ++--- 5 files changed, 23 insertions(+), 23 deletions(-) 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 7459f0ebafe..8e2b863ed66 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -80,7 +80,8 @@ Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { // Updating the motion command LinearMotionInstant target{ball_position - offset_from_ball}; - MotionCommand modified_command{"path_target", target, FacePoint{plan_request.motion_command.target.position}}; + 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 @@ -91,18 +92,19 @@ Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { 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 + // 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}; + 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 @@ -112,8 +114,7 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { } void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_instant) { - if (current_state_ == INITIAL_APPROACH && - (path_target_.is_done())) { + if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) { current_state_ = FINAL_APPROACH; } } 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 4349e43c9f1..75ba5ffdbc7 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -2,10 +2,10 @@ #include +#include "planning/planner/collect_path_planner.hpp" #include "planning/planner/path_planner.hpp" -#include "planning/trajectory.hpp" #include "planning/planner/path_target_path_planner.hpp" -#include "planning/planner/collect_path_planner.hpp" +#include "planning/trajectory.hpp" class Configuration; class ConfigDouble; @@ -38,9 +38,7 @@ class LineKickPathPlanner : public PathPlanner { [[nodiscard]] bool is_done() const override; private: - enum State { - INITIAL_APPROACH, FINAL_APPROACH - }; + enum State { INITIAL_APPROACH, FINAL_APPROACH }; State current_state_ = INITIAL_APPROACH; PathTargetPathPlanner path_target_{}; CollectPathPlanner collect_planner_{}; @@ -51,10 +49,12 @@ class LineKickPathPlanner : public PathPlanner { bool average_ball_vel_initialized_ = false; std::optional target_kick_pos_; - // Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles); + // 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); + // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet + // static_obstacles, std::vector dynamic_obstacles); void state_transition(BallState ball, RobotInstant start_instant); // PlayState::State current_state_; diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index d838b3b038e..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 + kAvoidBallDistance); + 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 5bcab63892f..80e0d8a184e 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -10,7 +10,6 @@ #include "../global_state.hpp" #include "context.hpp" -#include "../global_state.hpp" #include "planning/dynamic_obstacle.hpp" #include "planning/instant.hpp" #include "planning/robot_constraints.hpp" @@ -30,9 +29,9 @@ struct PlanRequest { PlanRequest(RobotInstant start, MotionCommand command, // NOLINT RobotConstraints constraints, rj_geometry::ShapeSet field_obstacles, rj_geometry::ShapeSet virtual_obstacles, TrajectoryCollection* planned_trajectories, - unsigned shell_id, const WorldState* world_state, PlayState play_state, int8_t priority = 0, - rj_drawing::RosDebugDrawer* debug_drawer = nullptr, bool ball_sense = false, - float min_dist_from_ball = 0, float dribbler_speed = 0) + unsigned shell_id, const WorldState* world_state, PlayState play_state, + int8_t priority = 0, rj_drawing::RosDebugDrawer* debug_drawer = nullptr, + bool ball_sense = false, float min_dist_from_ball = 0, float dribbler_speed = 0) : start(start), motion_command(command), // NOLINT constraints(constraints), @@ -112,6 +111,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. */ @@ -121,7 +121,6 @@ struct PlanRequest { * Dribbler Speed */ float dribbler_speed = 0; - }; /** diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 8d27472e089..09d6583268c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -87,7 +87,6 @@ Offense::State Offense::update_state() { } std::optional Offense::state_to_task(RobotIntent intent) { - SPDLOG_INFO(current_state_); if (current_state_ == IDLING) { @@ -140,8 +139,8 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.is_active = true; // intent.motion_command = planning::MotionCommand{ - // "path_target", planning::LinearMotionInstant{last_world_state_->ball.position, {0.0}}, - // planning::FaceBall{}}; + // "path_target", planning::LinearMotionInstant{last_world_state_->ball.position, + // {0.0}}, planning::FaceBall{}}; return intent; } else if (current_state_ == RECEIVING) { From 4cc25bd9d41aff7a41f8192ba33365012ce8faf9 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 22 Jan 2024 02:18:10 +0000 Subject: [PATCH 11/15] fixed compiler error and targets 7/8 part of goal --- .../planner/line_kick_path_planner.cpp | 24 +++++++++---------- .../strategy/agent/position/offense.cpp | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) 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 8e2b863ed66..ac4a4189119 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -47,19 +47,19 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { // make_shared(ball.predict_at(cur_time).position, ball_avoid_distance)); // // only plan line kick if not is_done - if (!this->is_done()) { - state_transition(ball, plan_request.start); - switch (current_state_) { - case INITIAL_APPROACH: - prev_path_ = initial(plan_request); - break; - case FINAL_APPROACH: - prev_path_ = final(plan_request); - break; - } - prev_path_.stamp(RJ::now()); - return prev_path_; + // if (!this->is_done()) { + state_transition(ball, plan_request.start); + switch (current_state_) { + case INITIAL_APPROACH: + prev_path_ = initial(plan_request); + break; + case FINAL_APPROACH: + prev_path_ = final(plan_request); + break; } + prev_path_.stamp(RJ::now()); + return prev_path_; + // } } Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 09d6583268c..24ce49b9480 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -129,7 +129,7 @@ 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(); - rj_geometry::Point scoring_point = their_goal_pos + field_dimensions_.goal_width() / 2.0; + 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; From b038f21a22eb9850ad6dcba5a3933ae255c489b5 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 21 Jan 2024 21:21:07 -0500 Subject: [PATCH 12/15] Fix Code Style On rewrite-linekicker (#2165) automated style fixes Co-authored-by: petergarud --- soccer/src/soccer/strategy/agent/position/offense.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 24ce49b9480..689e50eda7d 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -129,7 +129,8 @@ 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(); - rj_geometry::Point scoring_point = their_goal_pos + field_dimensions_.goal_width() * 3.0 / 8.0; + 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; From 50cab811b3ef5faaf00213037f07d502a5db0e87 Mon Sep 17 00:00:00 2001 From: petergarud Date: Wed, 24 Jan 2024 00:41:43 +0000 Subject: [PATCH 13/15] resolved pr comments --- .../planner/line_kick_path_planner.cpp | 38 +++++++------------ .../planner/line_kick_path_planner.hpp | 11 +++++- 2 files changed, 22 insertions(+), 27 deletions(-) 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 ac4a4189119..1029e52c72d 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -13,16 +13,12 @@ using namespace rj_geometry; namespace planning { Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { - SPDLOG_INFO("Robot {} state {}", plan_request.shell_id, current_state_); - if (plan_request.play_state_ == PlayState::halt() || plan_request.play_state_ == PlayState::stop()) { return Trajectory{}; } const BallState& ball = plan_request.world_state->ball; - // const MotionCommand& command = plan_request.motion_command; - // const RobotInstant& start_instant = plan_request.start; if (!average_ball_vel_initialized_) { average_ball_vel_ = ball.velocity; @@ -37,18 +33,10 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8); } - // 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)); - - // // only plan line kick if not is_done - // if (!this->is_done()) { - state_transition(ball, plan_request.start); + // only process state transition if already started the planning + if (has_created_plan) { + process_state_transition(); + } switch (current_state_) { case INITIAL_APPROACH: prev_path_ = initial(plan_request); @@ -58,21 +46,21 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { break; } prev_path_.stamp(RJ::now()); + has_created_plan = true; return prev_path_; - // } } Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { - // // Getting ball info + // Getting ball info const BallState& ball = plan_request.world_state->ball; - // // Creating a modified plan_request to send to PathTargetPlanner + // Creating a modified plan_request to send to PathTargetPlanner PlanRequest modified_request = plan_request; // Where to navigate to - auto distance_from_ball = kBallRadius + kRobotRadius + 0.05; + auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy; - auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{0.5}).position; + auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position; auto goal_to_ball = (plan_request.motion_command.target.position - ball_position); auto offset_from_ball = distance_from_ball * goal_to_ball.normalized(); @@ -95,12 +83,12 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { const BallState& ball = plan_request.world_state->ball; - // // Creating a modified plan_request to send to PathTargetPlanner + // 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; + auto vel = goal_to_ball.normalized() * kFinalRobotSpeed; LinearMotionInstant target{ball.position, vel}; MotionCommand modified_command{"path_target", target, @@ -113,7 +101,7 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { return path; } -void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_instant) { +void LineKickPathPlanner::process_state_transition() { if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) { current_state_ = FINAL_APPROACH; } @@ -122,7 +110,7 @@ void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_in 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 75ba5ffdbc7..0278687330b 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -34,6 +34,7 @@ class LineKickPathPlanner : public PathPlanner { target_kick_pos_ = std::nullopt; current_state_ = INITIAL_APPROACH; average_ball_vel_initialized_ = false; + has_created_plan = false; } [[nodiscard]] bool is_done() const override; @@ -44,9 +45,15 @@ class LineKickPathPlanner : public PathPlanner { CollectPathPlanner collect_planner_{}; Trajectory prev_path_; - static constexpr double IS_DONE_BALL_VEL = 1.5; + // 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, @@ -55,7 +62,7 @@ class LineKickPathPlanner : public PathPlanner { Trajectory final(const PlanRequest& plan_request); // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet // static_obstacles, std::vector dynamic_obstacles); - void state_transition(BallState ball, RobotInstant start_instant); + void process_state_transition(); // PlayState::State current_state_; }; From 5751b97ed8c0f602331820abb501c543dd888310 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Fri, 26 Jan 2024 19:27:22 -0500 Subject: [PATCH 14/15] Fix Code Style On rewrite-linekicker (#2169) automated style fixes Co-authored-by: sid-parikh --- soccer/src/soccer/planning/planner/line_kick_path_planner.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0278687330b..0e451c79764 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -48,7 +48,7 @@ class LineKickPathPlanner : public PathPlanner { // 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 kPredictIn = 0.5; // seconds static constexpr double kAvoidBallBy = 0.05; rj_geometry::Point average_ball_vel_; From 2ff2b875a8a32a5711b9bd2fd86f38f0a60f8314 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sat, 27 Jan 2024 00:48:59 +0000 Subject: [PATCH 15/15] i'm bad at git --- soccer/src/soccer/planning/planner/line_kick_path_planner.cpp | 4 ++-- soccer/src/soccer/planning/planner/plan_request.hpp | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) 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 1029e52c72d..f12219ffcf4 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -13,8 +13,8 @@ using namespace rj_geometry; namespace planning { Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { - if (plan_request.play_state_ == PlayState::halt() || - plan_request.play_state_ == PlayState::stop()) { + if (plan_request.play_state == PlayState::halt() || + plan_request.play_state == PlayState::stop()) { return Trajectory{}; } diff --git a/soccer/src/soccer/planning/planner/plan_request.hpp b/soccer/src/soccer/planning/planner/plan_request.hpp index 80e0d8a184e..54eb9a0ffa5 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -41,7 +41,6 @@ struct PlanRequest { shell_id(shell_id), priority(priority), world_state(world_state), - play_state_(play_state), play_state(play_state), debug_drawer(debug_drawer), ball_sense(ball_sense),