From cd8ff842f558ad0c5dde3e390cd0a32b1ffde484 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 19 Jan 2025 21:47:21 -0500 Subject: [PATCH 01/12] good colelct --- .../planning/planner/collect_path_planner.cpp | 298 +++++++++++++++++- .../planning/planner/collect_path_planner.hpp | 25 +- .../planning/planner/settle_path_planner.cpp | 9 +- .../planning/primitives/angle_planning.cpp | 5 + .../strategy/agent/position/solo_offense.cpp | 4 +- 5 files changed, 320 insertions(+), 21 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index cd42ccdfe7f..0a99c043035 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -103,18 +103,23 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { } // Check if we should transition to control from approach - process_state_transition(ball, start_instant); + process_state_transition(plan_request, ball, start_instant); // List of obstacles ShapeSet static_obstacles; std::vector dynamic_obstacles; - fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false); + + if (current_state_ == Intercept) { + fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, true); + } else { + Trajectory ball; + fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false, &ball); + } // Return an empty trajectory if the ball is hitting static obstacles // or it is in the goalie area. // Check the robot for the same conditions. - if (static_obstacles.hit(ball.position) || - static_obstacles.hit(start_instant.pose.position())) { + if (static_obstacles.hit(start_instant.pose.position())) { return Trajectory{}; } @@ -134,11 +139,20 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { previous_ = control(plan_request, partial_start_instant, partial_path, static_obstacles, dynamic_obstacles); break; + case Intercept: { + Point delta_pos; + Point face_pos; + + calc_delta_pos_for_dir(ball, start_instant, &delta_pos, &face_pos); + previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles, delta_pos, face_pos); + break; + } default: previous_ = invalid(plan_request, static_obstacles, dynamic_obstacles); break; } + return previous_; } @@ -156,7 +170,16 @@ void CollectPathPlanner::check_solution_validity(BallState ball, RobotInstant st } } -void CollectPathPlanner::process_state_transition(BallState ball, RobotInstant start_instant) { +void CollectPathPlanner::process_state_transition(const PlanRequest& request, BallState ball, RobotInstant start_instant) { + + // If the ball is moving, intercept + // if not, regularly approach + if (current_state_ == CoarseApproach && average_ball_vel_.mag() > 0.2) { + current_state_ = Intercept; + } else if (current_state_ == Intercept && average_ball_vel_.mag() < 0.2) { + current_state_ = CoarseApproach; + } + // Do the transitions double dist = (start_instant.position() - ball.position).mag() - kRobotMouthRadius; double speed_diff = (start_instant.linear_velocity() - average_ball_vel_).mag() - @@ -164,7 +187,7 @@ void CollectPathPlanner::process_state_transition(BallState ball, RobotInstant s // If we are in range to the slow dist if (dist < collect::PARAM_approach_dist_target + kRobotMouthRadius && - current_state_ == CoarseApproach) { + (current_state_ == CoarseApproach || current_state_ == Intercept)) { current_state_ = FineApproach; } @@ -177,8 +200,7 @@ void CollectPathPlanner::process_state_transition(BallState ball, RobotInstant s // If we are close enough to the target point near the ball // and almost the same speed we want, start slowing down // TODO(#1518): Check for ball sense? - if (dist < collect::PARAM_dist_cutoff_to_control && - speed_diff < collect::PARAM_vel_cutoff_to_control && current_state_ == FineApproach) { + if (request.ball_sense && current_state_ == FineApproach) { current_state_ = Control; } } @@ -221,13 +243,14 @@ Trajectory CollectPathPlanner::coarse_approach( LinearMotionInstant target_slow{path_coarse_target_, target_slow_vel}; Replanner::PlanParams params{start, - target_slow, - static_obstacles, - dynamic_obstacles, - plan_request.field_dimensions, - plan_request.constraints, - AngleFns::face_point(ball.position), - plan_request.shell_id}; + target_slow, + static_obstacles, + dynamic_obstacles, + plan_request.field_dimensions, + plan_request.constraints, + AngleFns::face_point(ball.position), + plan_request.shell_id}; + Trajectory coarse_path = Replanner::create_plan(params, previous_); if (plan_request.debug_drawer != nullptr) { @@ -243,6 +266,246 @@ Trajectory CollectPathPlanner::coarse_approach( return coarse_path; } +void CollectPathPlanner::calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, + rj_geometry::Point* delta_robot_pos, + rj_geometry::Point* face_pos) { + // Get angle between target and normal hit + Point normal_face_vector = ball.position - start_instant.position(); + + // Since we loose the sign for the angle between call, there are two + // possibilities + + *delta_robot_pos = Point(0, 0); + *face_pos = start_instant.position() + + Point::direction(normal_face_vector.angle()) * 10; +} + +Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles, + rj_geometry::Point delta_pos, rj_geometry::Point face_pos) { + + const double max_ball_angle_change_for_path_reset = + settle::PARAM_max_ball_angle_for_reset * M_PI / 180.0f; + + BallState ball = plan_request.world_state->ball; + + // If the ball changed directions or magnitude really quickly, do a reset of + // target + if (average_ball_vel_.angle_between(ball.velocity) > max_ball_angle_change_for_path_reset || + (average_ball_vel_ - ball.velocity).mag() > settle::PARAM_max_ball_vel_for_path_reset) { + first_intercept_target_found_ = false; + average_ball_vel_initialized_ = false; + } + + // Try find best point to intercept using brute force method + // where we check ever X distance along the ball velocity vector + // + // Disallow points outside the field + const Rect& field_rect = FieldDimensions::current_dimensions.field_rect(); + + std::optional ball_intercept_maybe; + RJ::Seconds best_buffer = RJ::Seconds(-1.0); + + int num_iterations = + std::ceil((settle::PARAM_search_end_dist - settle::PARAM_search_start_dist) / + settle::PARAM_search_inc_dist); + + for (int iteration = 0; iteration < num_iterations; iteration++) { + double dist = settle::PARAM_search_start_dist + iteration * settle::PARAM_search_inc_dist; + // Time for ball to reach the target point + std::optional maybe_ball_time = ball.query_seconds_to_dist(dist); + + if (!maybe_ball_time.has_value()) { + break; + } + + RJ::Seconds ball_time = maybe_ball_time.value(); + + // Account for the target point causing a slight offset in robot + // position since we want the ball to still hit the mouth + Point ball_vel_intercept = + ball.position + average_ball_vel_.normalized() * dist + delta_pos; + + if (!field_rect.contains_point(ball_vel_intercept)) { + break; + } + + // Use the mouth to center vector, rotate by X degrees + // Take the delta between old and new mouth vector and move + // target_robot_intersection by that amount + // It should be about stopped at that location. + // Could add a little backwards motion, but it isn't as clean in the + // planning side + LinearMotionInstant target_robot_intersection{ball_vel_intercept, Point()}; + + // Plan a path from our partial path start location to the intercept + // test location + Trajectory path = CreatePath::intermediate( + start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot, + start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + plan_request.shell_id); + + // Calculate the + RJ::Seconds buffer_duration = ball_time - path.duration(); + if (!path.empty() && buffer_duration > best_buffer) { + ball_intercept_maybe = ball_vel_intercept; + best_buffer = buffer_duration; + } + + // If valid path to location + // and we can reach the target point before ball + // + // Don't do the average here so we can project the intercept point + // inside the field + if (!path.empty() && best_buffer > RJ::Seconds(settle::PARAM_intercept_buffer_time)) { + break; + } + } + + rj_geometry::Point ball_vel_intercept; + // If we still haven't found a valid intercept point, just target the stop + // point. + if (ball_intercept_maybe.has_value()) { + ball_vel_intercept = ball_intercept_maybe.value(); + } else { + ball_vel_intercept = ball.query_stop_position() + delta_pos; + } + + // Make sure target_robot_intersection is inside the field + // If not, project it into the field + if (!field_rect.contains_point(ball_vel_intercept)) { + auto intersect_return = field_rect.intersects(Segment(ball.position, ball_vel_intercept)); + + bool valid_intersect = std::get<0>(intersect_return); + std::vector intersect_pts = std::get<1>(intersect_return); + + // If the ball intersects the field at some point + // Just get the intersect point as the new target + if (valid_intersect) { + // Sorts based on distance to intercept target + // The closest one is the intercept point which the ball moves + // through leaving the field Not the one on the other side of the + // field + sort(intersect_pts.begin(), intersect_pts.end(), + [ball_vel_intercept](Point a, Point b) { + return (a - ball_vel_intercept).mag() < (b - ball_vel_intercept).mag(); + }); + + // Choose a point just inside the field + // Add in the delta_pos for weird target angles since the math is + // not super fun and not really needed + ball_vel_intercept = intersect_pts.at(0) + delta_pos; + + // Doesn't intersect + // project the ball into the field + } else { + // Simple projection + ball_vel_intercept.x() = std::max(ball_vel_intercept.x(), (double)field_rect.minx()); + ball_vel_intercept.x() = std::min(ball_vel_intercept.x(), (double)field_rect.maxx()); + + ball_vel_intercept.y() = std::max(ball_vel_intercept.y(), (double)field_rect.miny()); + ball_vel_intercept.y() = std::min(ball_vel_intercept.y(), (double)field_rect.maxy()); + } + } + + // Could not find a valid path that reach the point first + // Just go for the farthest point and recalc next time + if (!first_intercept_target_found_) { + avg_instantaneous_intercept_target_ = ball_vel_intercept; + path_intercept_target_ = ball_vel_intercept; + + first_intercept_target_found_ = true; + } else { + avg_instantaneous_intercept_target_ = + apply_low_pass_filter(avg_instantaneous_intercept_target_, ball_vel_intercept, + settle::PARAM_target_point_gain); + } + + // Shortcuts the crazy path planner to just move into the path of the ball + // if we are very close Only shortcuts if the target point is further up the + // path than we are going to hit AKA only shortcut if we have to move + // backwards along the path to capture the ball + // + // Still want to do the math in case the best point changes + // which happens a lot when the ball is first kicked + + // If we are within a single radius of the ball path + // and in front of it + // just move directly to the path location + Segment ball_line = Segment( + ball.position, ball.position + average_ball_vel_.norm() * settle::PARAM_search_end_dist); + Point closest_pt = ball_line.nearest_point(start_instant.position()) + delta_pos; + + Point ball_to_pt_dir = closest_pt - ball.position; + bool in_front_of_ball = average_ball_vel_.angle_between(ball_to_pt_dir) < 3.14 / 2; + + // Only force a direct movement if we are within a small range AND + // we have run the algorithm at least once AND + // the target point found in the algorithm is further than we are or just + // about equal + if (in_front_of_ball && + (closest_pt - start_instant.position()).mag() < settle::PARAM_shortcut_dist && + first_intercept_target_found_ && + (closest_pt - ball.position).mag() - + (avg_instantaneous_intercept_target_ - ball.position).mag() < + settle::PARAM_shortcut_dist) { + LinearMotionInstant target{closest_pt, + settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_}; + + Trajectory shortcut = CreatePath::intermediate( + start_instant.linear_motion(), target, plan_request.constraints.mot, + start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + plan_request.shell_id); + + if (!shortcut.empty()) { + plan_angles(&shortcut, start_instant, AngleFns::face_point(face_pos), + plan_request.constraints.rot); + shortcut.stamp(RJ::now()); + return shortcut; + } + } + + // There's some major problems with repeatedly changing the target for the + // path planner To alleviate this problem, we only change the target point + // when it moves over X amount from the previous path target + // + // This combined with the shortcut is guaranteed to get in front of the ball + // correctly If not, add some sort of distance scale that changes based on + // how close the robot is to the target + if ((path_intercept_target_ - avg_instantaneous_intercept_target_).mag() > kRobotMouthRadius) { + path_intercept_target_ = avg_instantaneous_intercept_target_; + } + + // Build a new path with the target + // Since the replanner exists, we don't have to deal with partial paths, + // just use the interface + LinearMotionInstant target_robot_intersection{ + path_intercept_target_, settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_}; + + Replanner::PlanParams params{start_instant, + target_robot_intersection, + static_obstacles, + dynamic_obstacles, + plan_request.field_dimensions, + plan_request.constraints, + AngleFns::face_point(face_pos), + plan_request.shell_id}; + Trajectory new_target_path = Replanner::create_plan(params, previous_); + + RJ::Seconds time_of_arrival = new_target_path.duration(); + new_target_path.set_debug_text(std::to_string(time_of_arrival.count()) + " s"); + + if (new_target_path.empty()) { + return previous_; + } + + plan_angles(&new_target_path, start_instant, AngleFns::face_point(face_pos), + plan_request.constraints.rot); + new_target_path.stamp(RJ::now()); + return new_target_path; +} + Trajectory CollectPathPlanner::fine_approach( const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, @@ -279,7 +542,6 @@ Trajectory CollectPathPlanner::fine_approach( // acceleration in the trapezoid motion_constraints_hit.max_speed = std::min(target_hit_vel.mag(), motion_constraints_hit.max_speed); - Replanner::PlanParams params{start_instant, target_hit, static_obstacles, @@ -289,8 +551,8 @@ Trajectory CollectPathPlanner::fine_approach( AngleFns::face_point(ball.position), plan_request.shell_id}; Trajectory path_hit = Replanner::create_plan(params, previous_); - path_hit.set_debug_text("fine"); + plan_angles(&path_hit, start_instant, AngleFns::face_point(ball.position), plan_request.constraints.rot); path_hit.stamp(RJ::now()); @@ -453,6 +715,8 @@ bool CollectPathPlanner::is_done() const { // (see process_state_transition()) if (current_state_ != Control) { return false; + } else { + return true; } // control state is done when the ball is slowed AND in mouth diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.hpp b/soccer/src/soccer/planning/planner/collect_path_planner.hpp index d2663f854e4..baa6809d5fb 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.hpp @@ -20,6 +20,8 @@ class CollectPathPlanner : public PathPlanner { // From start of subbehavior to the start of the slow part of the // approach CoarseApproach, + // Intercepts a moving ball + Intercept, // From the slow part of the approach to the touching of the ball FineApproach, // From touching the ball to stopped with the ball in the mouth @@ -39,13 +41,19 @@ class CollectPathPlanner : public PathPlanner { // and won't intercept ball correctly anymore void check_solution_validity(BallState ball, RobotInstant start); - void process_state_transition(BallState ball, RobotInstant start_instant); + void process_state_transition(const PlanRequest& request, BallState ball, RobotInstant start_instant); Trajectory coarse_approach( const PlanRequest& plan_request, RobotInstant start, const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles); + Trajectory intercept(const PlanRequest& plan_request, + RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles, + rj_geometry::Point delta_pos, rj_geometry::Point face_pos); + Trajectory fine_approach( const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, @@ -60,6 +68,11 @@ class CollectPathPlanner : public PathPlanner { const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles); + // Calculate the delta position to get the robot in the correct location + // And the face point to get the bounce right towards their goal + void calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, rj_geometry::Point* delta_robot_pos, + rj_geometry::Point* face_pos); + Trajectory previous_; CollectPathPathPlannerStates current_state_ = CollectPathPathPlannerStates::CoarseApproach; @@ -76,10 +89,20 @@ class CollectPathPlanner : public PathPlanner { rj_geometry::Point path_coarse_target_; bool path_coarse_target_initialized_ = false; + // Intercept Target Filtering Variables + rj_geometry::Point avg_instantaneous_intercept_target_; + bool first_intercept_target_found_ = false; + + // Only change the target of the path if it changes significantly + rj_geometry::Point path_intercept_target_; + // is_done vars std::optional cached_start_instant_; std::optional cached_robot_pos_; std::optional cached_ball_pos_; + + // The direction to bounce the intercept to + rj_geometry::Point target_bounce_direction_; }; } // namespace planning diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 04c78245030..2c454d576b2 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -467,8 +467,15 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta // Target stopping point with 0 speed. LinearMotionInstant final_stopping_motion{final_stopping_point}; - Trajectory dampen_end = CreatePath::simple(start_instant.linear_motion(), final_stopping_motion, + Trajectory dampen_end; + + if (previous_.empty()) { + dampen_end = CreatePath::simple(start_instant.linear_motion(), final_stopping_motion, plan_request.constraints.mot, start_instant.stamp); + } else { + dampen_end = CreatePath::simple(previous_.instant_at(previous_.num_instants() - 1).linear_motion(), final_stopping_motion, + plan_request.constraints.mot, start_instant.stamp); + } dampen_end.set_debug_text("Damping"); diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 82adb2b338b..38fa91fcc06 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -76,6 +76,11 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, target_angles.at(i) = angle_function(instant, target_angles.at(i - 1), &gradient); velocity.at(i) = trajectory->instant_at(i).linear_velocity().dot(rj_geometry::Point(gradient)); + if (std::isnan(velocity.at(i))) { + SPDLOG_INFO("Linear Velocity: {}", trajectory->instant_at(i).linear_velocity().mag()); + SPDLOG_INFO("Gradient: {}", rj_geometry::Point(gradient).mag()); + SPDLOG_INFO("SDFSDFSDFSDFSDFSDF"); + } } // TODO(#1506): Re-enable this. Currently the forward-tracking is disabled diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 54a10d5bf83..872994b4d4e 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -100,7 +100,7 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { planning::LinearMotionInstant target{ last_world_state_->get_robot(true, robot_id_).pose.position() + robotToBall}; auto pivot_cmd = - planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true}; + planning::MotionCommand{"collect"}; intent.motion_command = pivot_cmd; intent.dribbler_speed = 255; return intent; @@ -123,7 +123,7 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { planning::LinearMotionInstant target{calculate_best_shot()}; // planning::LinearMotionInstant target{last_world_state_->ball.position}; auto kick_cmd = - planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true}; + planning::MotionCommand{"line_kick", target, planning::FaceTarget{}, true}; intent.motion_command = kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; From 00e6ef9e69d223bf5e9ca6782fec322c39f49494 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 19 Jan 2025 21:49:29 -0500 Subject: [PATCH 02/12] Fix Code Style On settle-collect (#2321) automated style fixes Co-authored-by: sanatd33 --- .../planning/planner/collect_path_planner.cpp | 48 +++++++++---------- .../planning/planner/collect_path_planner.hpp | 10 ++-- .../planning/planner/settle_path_planner.cpp | 9 ++-- .../strategy/agent/position/solo_offense.cpp | 3 +- 4 files changed, 35 insertions(+), 35 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 0a99c043035..2e5cedf1d7d 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -142,9 +142,10 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { case Intercept: { Point delta_pos; Point face_pos; - + calc_delta_pos_for_dir(ball, start_instant, &delta_pos, &face_pos); - previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles, delta_pos, face_pos); + previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles, + delta_pos, face_pos); break; } default: @@ -152,7 +153,6 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { break; } - return previous_; } @@ -170,8 +170,8 @@ void CollectPathPlanner::check_solution_validity(BallState ball, RobotInstant st } } -void CollectPathPlanner::process_state_transition(const PlanRequest& request, BallState ball, RobotInstant start_instant) { - +void CollectPathPlanner::process_state_transition(const PlanRequest& request, BallState ball, + RobotInstant start_instant) { // If the ball is moving, intercept // if not, regularly approach if (current_state_ == CoarseApproach && average_ball_vel_.mag() > 0.2) { @@ -243,14 +243,14 @@ Trajectory CollectPathPlanner::coarse_approach( LinearMotionInstant target_slow{path_coarse_target_, target_slow_vel}; Replanner::PlanParams params{start, - target_slow, - static_obstacles, - dynamic_obstacles, - plan_request.field_dimensions, - plan_request.constraints, - AngleFns::face_point(ball.position), - plan_request.shell_id}; - + target_slow, + static_obstacles, + dynamic_obstacles, + plan_request.field_dimensions, + plan_request.constraints, + AngleFns::face_point(ball.position), + plan_request.shell_id}; + Trajectory coarse_path = Replanner::create_plan(params, previous_); if (plan_request.debug_drawer != nullptr) { @@ -267,8 +267,8 @@ Trajectory CollectPathPlanner::coarse_approach( } void CollectPathPlanner::calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, - rj_geometry::Point* delta_robot_pos, - rj_geometry::Point* face_pos) { + rj_geometry::Point* delta_robot_pos, + rj_geometry::Point* face_pos) { // Get angle between target and normal hit Point normal_face_vector = ball.position - start_instant.position(); @@ -276,18 +276,18 @@ void CollectPathPlanner::calc_delta_pos_for_dir(BallState ball, RobotInstant sta // possibilities *delta_robot_pos = Point(0, 0); - *face_pos = start_instant.position() + - Point::direction(normal_face_vector.angle()) * 10; + *face_pos = start_instant.position() + Point::direction(normal_face_vector.angle()) * 10; } -Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, RobotInstant start_instant, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles, - rj_geometry::Point delta_pos, rj_geometry::Point face_pos) { - +Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, + RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles, + rj_geometry::Point delta_pos, + rj_geometry::Point face_pos) { const double max_ball_angle_change_for_path_reset = settle::PARAM_max_ball_angle_for_reset * M_PI / 180.0f; - + BallState ball = plan_request.world_state->ball; // If the ball changed directions or magnitude really quickly, do a reset of @@ -297,7 +297,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, RobotI first_intercept_target_found_ = false; average_ball_vel_initialized_ = false; } - + // Try find best point to intercept using brute force method // where we check ever X distance along the ball velocity vector // diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.hpp b/soccer/src/soccer/planning/planner/collect_path_planner.hpp index baa6809d5fb..eddb405fb65 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.hpp @@ -41,15 +41,15 @@ class CollectPathPlanner : public PathPlanner { // and won't intercept ball correctly anymore void check_solution_validity(BallState ball, RobotInstant start); - void process_state_transition(const PlanRequest& request, BallState ball, RobotInstant start_instant); + void process_state_transition(const PlanRequest& request, BallState ball, + RobotInstant start_instant); Trajectory coarse_approach( const PlanRequest& plan_request, RobotInstant start, const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles); - Trajectory intercept(const PlanRequest& plan_request, - RobotInstant start_instant, + Trajectory intercept(const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles, rj_geometry::Point delta_pos, rj_geometry::Point face_pos); @@ -70,8 +70,8 @@ class CollectPathPlanner : public PathPlanner { // Calculate the delta position to get the robot in the correct location // And the face point to get the bounce right towards their goal - void calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, rj_geometry::Point* delta_robot_pos, - rj_geometry::Point* face_pos); + void calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, + rj_geometry::Point* delta_robot_pos, rj_geometry::Point* face_pos); Trajectory previous_; diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 2c454d576b2..2bde6e3501d 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -468,13 +468,14 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta LinearMotionInstant final_stopping_motion{final_stopping_point}; Trajectory dampen_end; - + if (previous_.empty()) { dampen_end = CreatePath::simple(start_instant.linear_motion(), final_stopping_motion, - plan_request.constraints.mot, start_instant.stamp); + plan_request.constraints.mot, start_instant.stamp); } else { - dampen_end = CreatePath::simple(previous_.instant_at(previous_.num_instants() - 1).linear_motion(), final_stopping_motion, - plan_request.constraints.mot, start_instant.stamp); + dampen_end = CreatePath::simple( + previous_.instant_at(previous_.num_instants() - 1).linear_motion(), + final_stopping_motion, plan_request.constraints.mot, start_instant.stamp); } dampen_end.set_debug_text("Damping"); diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 872994b4d4e..de80ddd5680 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -99,8 +99,7 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { robotToBall = robotToBall.normalized(length); planning::LinearMotionInstant target{ last_world_state_->get_robot(true, robot_id_).pose.position() + robotToBall}; - auto pivot_cmd = - planning::MotionCommand{"collect"}; + auto pivot_cmd = planning::MotionCommand{"collect"}; intent.motion_command = pivot_cmd; intent.dribbler_speed = 255; return intent; From e95f853b61215222373343f115e897a7b8dc59eb Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 26 Jan 2025 21:22:43 -0500 Subject: [PATCH 03/12] settle nonesense --- .../planning/planner/settle_path_planner.cpp | 5 ++--- soccer/src/soccer/planning/trajectory.cpp | 7 +++---- .../soccer/strategy/agent/position/offense.cpp | 16 +++++++++------- .../agent/position/robot_factory_position.cpp | 6 +++--- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 2bde6e3501d..cd85f74c5d7 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -473,9 +473,8 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta dampen_end = CreatePath::simple(start_instant.linear_motion(), final_stopping_motion, plan_request.constraints.mot, start_instant.stamp); } else { - dampen_end = CreatePath::simple( - previous_.instant_at(previous_.num_instants() - 1).linear_motion(), - final_stopping_motion, plan_request.constraints.mot, start_instant.stamp); + dampen_end = CreatePath::simple(previous_.last().linear_motion(), final_stopping_motion, + plan_request.constraints.mot, previous_.last().stamp); } dampen_end.set_debug_text("Damping"); diff --git a/soccer/src/soccer/planning/trajectory.cpp b/soccer/src/soccer/planning/trajectory.cpp index 89f48864716..9f7884cfce9 100644 --- a/soccer/src/soccer/planning/trajectory.cpp +++ b/soccer/src/soccer/planning/trajectory.cpp @@ -23,12 +23,11 @@ Trajectory::Trajectory(Trajectory a, const Trajectory& b) { using rj_geometry::Point; if (!a_end.position().near_point(b_begin.position(), 1e-6) || - !a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-6) || a_end.stamp != b_begin.stamp) { SPDLOG_ERROR("points near? {}, vels near? {}, timestamps match? {}", - !a_end.position().near_point(b_begin.position(), 1e-6), - !a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-6), - a_end.stamp != b_begin.stamp); + a_end.position().near_point(b_begin.position(), 1e-6), + a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-6), + a_end.stamp == b_begin.stamp); throw std::invalid_argument( "Cannot splice trajectories a and b, where a.last() != b.first()"); } diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 3daf2fb1bc3..de0d7ddb2cd 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -60,9 +60,9 @@ Offense::State Offense::next_state() { case POSSESSION_START: { // If we can make a shot, take it // If we need to stop possessing now, shoot. - if (has_open_shot() || timed_out()) { - return SHOOTING_START; - } + // if (has_open_shot() || timed_out()) { + // return SHOOTING_START; + // } // No open shot, try to pass. // This will trigger an automatic switch to passing if a pass is @@ -75,9 +75,10 @@ Offense::State Offense::next_state() { case POSSESSION: { // If we can make a shot, make it. // If we need to stop possessing now, shoot. - if (has_open_shot() || timed_out()) { - return SHOOTING_START; - } + + // if (has_open_shot() || timed_out()) { + // return SHOOTING_START; + // } return POSSESSION; } @@ -294,7 +295,8 @@ std::optional Offense::state_to_task(RobotIntent intent) { // intent.motion_command = settle_cmd; // intent.dribbler_speed = 255.0; // } else { - auto collect_cmd = planning::MotionCommand{"collect"}; + auto collect_cmd = planning::MotionCommand{"settle"}; + collect_cmd.target = planning::LinearMotionInstant{last_world_state_->ball.position}; intent.motion_command = collect_cmd; intent.dribbler_speed = 255.0; // } diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 7634ff96220..1db257544ec 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -262,10 +262,10 @@ void RobotFactoryPosition::set_default_position() { field_dimensions_.center_field_loc().y() - kBallDiameter) { // Offensive mode // Closest 2 robots on defense, rest on offense - if (i <= 3) { + if (i <= 2) { set_current_position(); } else { - set_current_position(); + set_current_position(); } } else { // Defensive mode @@ -273,7 +273,7 @@ void RobotFactoryPosition::set_default_position() { if (i <= 3) { set_current_position(); } else { - set_current_position(); + set_current_position(); } } } From 6fb75a191c66f473970edb66c21e3602fe83ca24 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 26 Jan 2025 21:41:22 -0500 Subject: [PATCH 04/12] Fix Code Style On settle-collect (#2324) automated style fixes Co-authored-by: sid-parikh --- soccer/src/soccer/planning/planner/settle_path_planner.cpp | 2 +- soccer/src/soccer/strategy/agent/position/offense.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index cd85f74c5d7..7f6899c6040 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -474,7 +474,7 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta plan_request.constraints.mot, start_instant.stamp); } else { dampen_end = CreatePath::simple(previous_.last().linear_motion(), final_stopping_motion, - plan_request.constraints.mot, previous_.last().stamp); + plan_request.constraints.mot, previous_.last().stamp); } dampen_end.set_debug_text("Damping"); diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index de0d7ddb2cd..faa22bdeac3 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -75,7 +75,7 @@ Offense::State Offense::next_state() { case POSSESSION: { // If we can make a shot, make it. // If we need to stop possessing now, shoot. - + // if (has_open_shot() || timed_out()) { // return SHOOTING_START; // } From 515d1bba112f573457f1eb21e3c355df825c64a6 Mon Sep 17 00:00:00 2001 From: sdhanyamraju6 Date: Tue, 28 Jan 2025 19:19:45 -0500 Subject: [PATCH 05/12] removed unnessesary change --- .../soccer/planning/primitives/angle_planning.cpp | 5 ----- .../src/soccer/strategy/agent/position/offense.cpp | 13 ++++++------- .../agent/position/robot_factory_position.cpp | 6 +++--- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 38fa91fcc06..82adb2b338b 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -76,11 +76,6 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, target_angles.at(i) = angle_function(instant, target_angles.at(i - 1), &gradient); velocity.at(i) = trajectory->instant_at(i).linear_velocity().dot(rj_geometry::Point(gradient)); - if (std::isnan(velocity.at(i))) { - SPDLOG_INFO("Linear Velocity: {}", trajectory->instant_at(i).linear_velocity().mag()); - SPDLOG_INFO("Gradient: {}", rj_geometry::Point(gradient).mag()); - SPDLOG_INFO("SDFSDFSDFSDFSDFSDF"); - } } // TODO(#1506): Re-enable this. Currently the forward-tracking is disabled diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index faa22bdeac3..f4b9a3af148 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -60,9 +60,9 @@ Offense::State Offense::next_state() { case POSSESSION_START: { // If we can make a shot, take it // If we need to stop possessing now, shoot. - // if (has_open_shot() || timed_out()) { - // return SHOOTING_START; - // } + if (has_open_shot() || timed_out()) { + return SHOOTING_START; + } // No open shot, try to pass. // This will trigger an automatic switch to passing if a pass is @@ -75,10 +75,9 @@ Offense::State Offense::next_state() { case POSSESSION: { // If we can make a shot, make it. // If we need to stop possessing now, shoot. - - // if (has_open_shot() || timed_out()) { - // return SHOOTING_START; - // } + if (has_open_shot() || timed_out()) { + return SHOOTING_START; + } return POSSESSION; } diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 1db257544ec..7634ff96220 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -262,10 +262,10 @@ void RobotFactoryPosition::set_default_position() { field_dimensions_.center_field_loc().y() - kBallDiameter) { // Offensive mode // Closest 2 robots on defense, rest on offense - if (i <= 2) { + if (i <= 3) { set_current_position(); } else { - set_current_position(); + set_current_position(); } } else { // Defensive mode @@ -273,7 +273,7 @@ void RobotFactoryPosition::set_default_position() { if (i <= 3) { set_current_position(); } else { - set_current_position(); + set_current_position(); } } } From 223c0d2584e83287a3580ac74c2025c1678f52b4 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 2 Feb 2025 21:58:05 -0500 Subject: [PATCH 06/12] started changes --- .../planning/planner/collect_path_planner.cpp | 84 +++++++------------ .../planning/planner/collect_path_planner.hpp | 21 ++--- soccer/src/soccer/planning/trajectory.cpp | 3 +- 3 files changed, 40 insertions(+), 68 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 2e5cedf1d7d..83e5f74e901 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -109,7 +109,7 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { ShapeSet static_obstacles; std::vector dynamic_obstacles; - if (current_state_ == Intercept) { + if (current_state_ == INTERCEPT) { fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, true); } else { Trajectory ball; @@ -125,27 +125,22 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { switch (current_state_) { // Moves from the current location to the slow point of approach - case CoarseApproach: + case COARSE_APPROACH: previous_ = coarse_approach(plan_request, start_instant, static_obstacles, dynamic_obstacles); break; // Moves from the slow point of approach to just before point of contact - case FineApproach: + case FINE_APPROACH: previous_ = fine_approach(plan_request, start_instant, static_obstacles, dynamic_obstacles); break; // Move through the ball and stop - case Control: + case CONTROL: previous_ = control(plan_request, partial_start_instant, partial_path, static_obstacles, dynamic_obstacles); break; - case Intercept: { - Point delta_pos; - Point face_pos; - - calc_delta_pos_for_dir(ball, start_instant, &delta_pos, &face_pos); - previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles, - delta_pos, face_pos); + case INTERCEPT: { + previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles); break; } default: @@ -163,8 +158,8 @@ void CollectPathPlanner::check_solution_validity(BallState ball, RobotInstant st // Check if we need to go back into approach // // See if we are not near the ball and both almost stopped - if (!near_ball && current_state_ == Control) { - current_state_ = CoarseApproach; + if (!near_ball && current_state_ == CONTROL) { + current_state_ = COARSE_APPROACH; approach_direction_created_ = false; control_path_created_ = false; } @@ -174,10 +169,10 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba RobotInstant start_instant) { // If the ball is moving, intercept // if not, regularly approach - if (current_state_ == CoarseApproach && average_ball_vel_.mag() > 0.2) { - current_state_ = Intercept; - } else if (current_state_ == Intercept && average_ball_vel_.mag() < 0.2) { - current_state_ = CoarseApproach; + if (current_state_ == COARSE_APPROACH && average_ball_vel_.mag() > kInterceptVelocityThreshold) { + current_state_ = INTERCEPT; + } else if (current_state_ == INTERCEPT && average_ball_vel_.mag() < kInterceptVelocityThreshold) { + current_state_ = COARSE_APPROACH; } // Do the transitions @@ -187,21 +182,20 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba // If we are in range to the slow dist if (dist < collect::PARAM_approach_dist_target + kRobotMouthRadius && - (current_state_ == CoarseApproach || current_state_ == Intercept)) { - current_state_ = FineApproach; + (current_state_ == COARSE_APPROACH || current_state_ == INTERCEPT)) { + current_state_ = FINE_APPROACH; } // If the ball gets knocked far away, go back to CoarseApproach if (dist > collect::PARAM_approach_dist_target + kRobotMouthRadius && - current_state_ == FineApproach) { - current_state_ = CoarseApproach; + current_state_ == FINE_APPROACH) { + current_state_ = COARSE_APPROACH; } // If we are close enough to the target point near the ball // and almost the same speed we want, start slowing down - // TODO(#1518): Check for ball sense? - if (request.ball_sense && current_state_ == FineApproach) { - current_state_ = Control; + if (request.ball_sense && current_state_ == FINE_APPROACH) { + current_state_ = CONTROL; } } @@ -266,30 +260,17 @@ Trajectory CollectPathPlanner::coarse_approach( return coarse_path; } -void CollectPathPlanner::calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, - rj_geometry::Point* delta_robot_pos, - rj_geometry::Point* face_pos) { - // Get angle between target and normal hit - Point normal_face_vector = ball.position - start_instant.position(); - - // Since we loose the sign for the angle between call, there are two - // possibilities - - *delta_robot_pos = Point(0, 0); - *face_pos = start_instant.position() + Point::direction(normal_face_vector.angle()) * 10; -} - Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles, - rj_geometry::Point delta_pos, - rj_geometry::Point face_pos) { + const std::vector& dynamic_obstacles) { const double max_ball_angle_change_for_path_reset = settle::PARAM_max_ball_angle_for_reset * M_PI / 180.0f; BallState ball = plan_request.world_state->ball; + rj_geometry::Point face_pos = start_instant.position() + Point::direction((ball.position - start_instant.position()).angle()) * 10; + // If the ball changed directions or magnitude really quickly, do a reset of // target if (average_ball_vel_.angle_between(ball.velocity) > max_ball_angle_change_for_path_reset || @@ -307,12 +288,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, std::optional ball_intercept_maybe; RJ::Seconds best_buffer = RJ::Seconds(-1.0); - int num_iterations = - std::ceil((settle::PARAM_search_end_dist - settle::PARAM_search_start_dist) / - settle::PARAM_search_inc_dist); - - for (int iteration = 0; iteration < num_iterations; iteration++) { - double dist = settle::PARAM_search_start_dist + iteration * settle::PARAM_search_inc_dist; + for (double dist = settle::PARAM_search_start_dist; dist < settle::PARAM_search_end_dist; dist += settle::PARAM_search_inc_dist) { // Time for ball to reach the target point std::optional maybe_ball_time = ball.query_seconds_to_dist(dist); @@ -325,7 +301,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, // Account for the target point causing a slight offset in robot // position since we want the ball to still hit the mouth Point ball_vel_intercept = - ball.position + average_ball_vel_.normalized() * dist + delta_pos; + ball.position + average_ball_vel_.normalized() * dist; if (!field_rect.contains_point(ball_vel_intercept)) { break; @@ -369,7 +345,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, if (ball_intercept_maybe.has_value()) { ball_vel_intercept = ball_intercept_maybe.value(); } else { - ball_vel_intercept = ball.query_stop_position() + delta_pos; + ball_vel_intercept = ball.query_stop_position(); } // Make sure target_robot_intersection is inside the field @@ -393,9 +369,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, }); // Choose a point just inside the field - // Add in the delta_pos for weird target angles since the math is - // not super fun and not really needed - ball_vel_intercept = intersect_pts.at(0) + delta_pos; + ball_vel_intercept = intersect_pts.at(0); // Doesn't intersect // project the ball into the field @@ -435,7 +409,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, // just move directly to the path location Segment ball_line = Segment( ball.position, ball.position + average_ball_vel_.norm() * settle::PARAM_search_end_dist); - Point closest_pt = ball_line.nearest_point(start_instant.position()) + delta_pos; + Point closest_pt = ball_line.nearest_point(start_instant.position()); Point ball_to_pt_dir = closest_pt - ball.position; bool in_front_of_ball = average_ball_vel_.angle_between(ball_to_pt_dir) < 3.14 / 2; @@ -681,7 +655,7 @@ Trajectory CollectPathPlanner::control(const PlanRequest& plan_request, RobotIns Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request, const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles) { - current_state_ = CoarseApproach; + current_state_ = COARSE_APPROACH; // Stop movement until next frame since it's the safest option // programmatically @@ -703,7 +677,7 @@ Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request, void CollectPathPlanner::reset() { previous_ = Trajectory(); - current_state_ = CollectPathPathPlannerStates::CoarseApproach; + current_state_ = CollectPathPathPlannerStates::COARSE_APPROACH; average_ball_vel_initialized_ = false; approach_direction_created_ = false; control_path_created_ = false; @@ -713,7 +687,7 @@ void CollectPathPlanner::reset() { bool CollectPathPlanner::is_done() const { // FSM: CoarseApproach -> FineApproach -> Control // (see process_state_transition()) - if (current_state_ != Control) { + if (current_state_ != CONTROL) { return false; } else { return true; diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.hpp b/soccer/src/soccer/planning/planner/collect_path_planner.hpp index eddb405fb65..7af6651def9 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.hpp @@ -19,13 +19,13 @@ class CollectPathPlanner : public PathPlanner { enum CollectPathPathPlannerStates { // From start of subbehavior to the start of the slow part of the // approach - CoarseApproach, + COARSE_APPROACH, // Intercepts a moving ball - Intercept, + INTERCEPT, // From the slow part of the approach to the touching of the ball - FineApproach, + FINE_APPROACH, // From touching the ball to stopped with the ball in the mouth - Control + CONTROL }; CollectPathPlanner() @@ -51,8 +51,7 @@ class CollectPathPlanner : public PathPlanner { Trajectory intercept(const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles, - rj_geometry::Point delta_pos, rj_geometry::Point face_pos); + const std::vector& dynamic_obstacles); Trajectory fine_approach( const PlanRequest& plan_request, RobotInstant start_instant, @@ -68,14 +67,9 @@ class CollectPathPlanner : public PathPlanner { const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles); - // Calculate the delta position to get the robot in the correct location - // And the face point to get the bounce right towards their goal - void calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, - rj_geometry::Point* delta_robot_pos, rj_geometry::Point* face_pos); - Trajectory previous_; - CollectPathPathPlannerStates current_state_ = CollectPathPathPlannerStates::CoarseApproach; + CollectPathPathPlannerStates current_state_ = CollectPathPathPlannerStates::COARSE_APPROACH; // Ball Velocity Filtering Variables rj_geometry::Point average_ball_vel_; @@ -103,6 +97,9 @@ class CollectPathPlanner : public PathPlanner { // The direction to bounce the intercept to rj_geometry::Point target_bounce_direction_; + + // Threshold for ball velocity to try to intercept; + double kInterceptVelocityThreshold = 0.2; }; } // namespace planning diff --git a/soccer/src/soccer/planning/trajectory.cpp b/soccer/src/soccer/planning/trajectory.cpp index 9f7884cfce9..1c9412e4c4e 100644 --- a/soccer/src/soccer/planning/trajectory.cpp +++ b/soccer/src/soccer/planning/trajectory.cpp @@ -23,10 +23,11 @@ Trajectory::Trajectory(Trajectory a, const Trajectory& b) { using rj_geometry::Point; if (!a_end.position().near_point(b_begin.position(), 1e-6) || + !a_end.linear_velocity().near_point(b_begin.linear_velocity, 1e-1) || a_end.stamp != b_begin.stamp) { SPDLOG_ERROR("points near? {}, vels near? {}, timestamps match? {}", a_end.position().near_point(b_begin.position(), 1e-6), - a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-6), + a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-1), a_end.stamp == b_begin.stamp); throw std::invalid_argument( "Cannot splice trajectories a and b, where a.last() != b.first()"); From 07bb99df8d1a7f744919b306cbdf2513d46e8500 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 2 Feb 2025 22:30:32 -0500 Subject: [PATCH 07/12] Fix Code Style On settle-collect (#2339) --- .../planning/planner/collect_path_planner.cpp | 16 ++++++++++------ soccer/src/soccer/planning/trajectory.cpp | 2 +- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 83e5f74e901..f6bfd75ebe3 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -169,9 +169,11 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba RobotInstant start_instant) { // If the ball is moving, intercept // if not, regularly approach - if (current_state_ == COARSE_APPROACH && average_ball_vel_.mag() > kInterceptVelocityThreshold) { + if (current_state_ == COARSE_APPROACH && + average_ball_vel_.mag() > kInterceptVelocityThreshold) { current_state_ = INTERCEPT; - } else if (current_state_ == INTERCEPT && average_ball_vel_.mag() < kInterceptVelocityThreshold) { + } else if (current_state_ == INTERCEPT && + average_ball_vel_.mag() < kInterceptVelocityThreshold) { current_state_ = COARSE_APPROACH; } @@ -269,7 +271,9 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, BallState ball = plan_request.world_state->ball; - rj_geometry::Point face_pos = start_instant.position() + Point::direction((ball.position - start_instant.position()).angle()) * 10; + rj_geometry::Point face_pos = + start_instant.position() + + Point::direction((ball.position - start_instant.position()).angle()) * 10; // If the ball changed directions or magnitude really quickly, do a reset of // target @@ -288,7 +292,8 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, std::optional ball_intercept_maybe; RJ::Seconds best_buffer = RJ::Seconds(-1.0); - for (double dist = settle::PARAM_search_start_dist; dist < settle::PARAM_search_end_dist; dist += settle::PARAM_search_inc_dist) { + for (double dist = settle::PARAM_search_start_dist; dist < settle::PARAM_search_end_dist; + dist += settle::PARAM_search_inc_dist) { // Time for ball to reach the target point std::optional maybe_ball_time = ball.query_seconds_to_dist(dist); @@ -300,8 +305,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, // Account for the target point causing a slight offset in robot // position since we want the ball to still hit the mouth - Point ball_vel_intercept = - ball.position + average_ball_vel_.normalized() * dist; + Point ball_vel_intercept = ball.position + average_ball_vel_.normalized() * dist; if (!field_rect.contains_point(ball_vel_intercept)) { break; diff --git a/soccer/src/soccer/planning/trajectory.cpp b/soccer/src/soccer/planning/trajectory.cpp index 1c9412e4c4e..45372c7aa14 100644 --- a/soccer/src/soccer/planning/trajectory.cpp +++ b/soccer/src/soccer/planning/trajectory.cpp @@ -23,7 +23,7 @@ Trajectory::Trajectory(Trajectory a, const Trajectory& b) { using rj_geometry::Point; if (!a_end.position().near_point(b_begin.position(), 1e-6) || - !a_end.linear_velocity().near_point(b_begin.linear_velocity, 1e-1) || + !a_end.linear_velocity().near_point(b_begin.linear_velocity, 1e-1) || a_end.stamp != b_begin.stamp) { SPDLOG_ERROR("points near? {}, vels near? {}, timestamps match? {}", a_end.position().near_point(b_begin.position(), 1e-6), From 97bce16f03667e3de2e92f999d14011fc3fa4b7a Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 4 Feb 2025 21:07:46 -0500 Subject: [PATCH 08/12] pr updates --- .../planning/planner/collect_path_planner.cpp | 173 ++---------------- .../planning/planner/collect_path_planner.hpp | 11 +- .../planning/planner/settle_path_planner.cpp | 17 +- .../planning/planner/settle_path_planner.hpp | 2 + .../planning/primitives/angle_planning.cpp | 6 + .../planning/primitives/create_path.cpp | 2 +- soccer/src/soccer/planning/trajectory.cpp | 2 +- .../strategy/agent/position/offense.cpp | 3 +- 8 files changed, 36 insertions(+), 180 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 83e5f74e901..841c13fd36e 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -50,10 +50,6 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { // How much of the previous path to steal const RJ::Seconds partial_replan_lead_time(Replanner::partial_replan_lead_time()); - // Check and see if we should reset the entire thing if we are super far off - // coarse or the ball state changes significantly - check_solution_validity(ball, start_instant); - // Change start instant to be the partial path end instead of the robot // current location if we actually have already calculated a path the frame // before @@ -102,13 +98,14 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { approach_direction_ = -average_ball_vel_.norm(); } - // Check if we should transition to control from approach + // Process the state transitions process_state_transition(plan_request, ball, start_instant); // List of obstacles ShapeSet static_obstacles; std::vector dynamic_obstacles; + // If we are intercepting, do add the ball as an obstacle if (current_state_ == INTERCEPT) { fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, true); } else { @@ -134,11 +131,7 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { previous_ = fine_approach(plan_request, start_instant, static_obstacles, dynamic_obstacles); break; - // Move through the ball and stop - case CONTROL: - previous_ = control(plan_request, partial_start_instant, partial_path, static_obstacles, - dynamic_obstacles); - break; + // Intercept a moving ball case INTERCEPT: { previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles); break; @@ -151,20 +144,6 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { return previous_; } -void CollectPathPlanner::check_solution_validity(BallState ball, RobotInstant start) { - bool near_ball = (ball.position - start.position()).mag() < - collect::PARAM_dist_cutoff_to_approach + collect::PARAM_dist_cutoff_to_control; - - // Check if we need to go back into approach - // - // See if we are not near the ball and both almost stopped - if (!near_ball && current_state_ == CONTROL) { - current_state_ = COARSE_APPROACH; - approach_direction_created_ = false; - control_path_created_ = false; - } -} - void CollectPathPlanner::process_state_transition(const PlanRequest& request, BallState ball, RobotInstant start_instant) { // If the ball is moving, intercept @@ -192,11 +171,8 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba current_state_ = COARSE_APPROACH; } - // If we are close enough to the target point near the ball - // and almost the same speed we want, start slowing down - if (request.ball_sense && current_state_ == FINE_APPROACH) { - current_state_ = CONTROL; - } + // If we are in FineApproach and we have the ball, terminate + is_ball_sense_ = request.ball_sense && current_state_ == FINE_APPROACH; } Trajectory CollectPathPlanner::coarse_approach( @@ -451,6 +427,11 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, path_intercept_target_ = avg_instantaneous_intercept_target_; } + if (start_instant.position().dist_to(ball.position) < start_instant.position().dist_to(path_intercept_target_) && + abs(average_ball_vel_.angle_to(start_instant.position() - ball.position)) < degrees_to_radians(60)) { + path_intercept_target_ = ball.position; + } + // Build a new path with the target // Since the replanner exists, we don't have to deal with partial paths, // just use the interface @@ -542,116 +523,6 @@ Trajectory CollectPathPlanner::fine_approach( return path_hit; } -Trajectory CollectPathPlanner::control(const PlanRequest& plan_request, RobotInstant start, - const Trajectory& /* partial_path */, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles) { - BallState ball = plan_request.world_state->ball; - RobotConstraints robot_constraints = plan_request.constraints; - MotionConstraints& motion_constraints = robot_constraints.mot; - - // Only plan the path once and run through it - // Otherwise it will basically push the ball across the field - if (control_path_created_ && !previous_.empty()) { - return previous_; - } - - control_path_created_ = true; - - // Scale the max acceleration so we don't stop too quickly - // Set the max speed to the current speed so it stays constant as - // we touch the ball and allows the dribbler to get some time to - // spin it up to speed - // Make sure we don't go over our current max speed - // Shouldn't happen (tm) - // - // If the ball is moving towards us (like receiving a pass) just move - // forward at touch_delta_speed - double current_speed = average_ball_vel_.mag() + collect::PARAM_touch_delta_speed; - - double velocity_scale = collect::PARAM_velocity_control_scale; - - // Moving at us - if (average_ball_vel_.angle_between((ball.position - start.position())) > 3.14 / 2) { - current_speed = collect::PARAM_touch_delta_speed; - velocity_scale = 0; - } - - motion_constraints.max_acceleration *= collect::PARAM_control_accel_scale; - motion_constraints.max_speed = std::min(current_speed, motion_constraints.max_speed); - - // Using the current velocity - // Calculate stopping distance given the acceleration - double max_accel = motion_constraints.max_acceleration; - - double non_zero_vel_time_delta = - collect::PARAM_approach_dist_target / collect::PARAM_touch_delta_speed; - - // Assuming const accel going to zero velocity - // speed / accel gives time to stop - // speed / 2 is average speed over entire operation - double stopping_dist = - collect::PARAM_approach_dist_target + current_speed * current_speed / (2 * max_accel); - - // Move through the ball some distance - // The initial part will be at a constant speed, then it will decelerate to - // 0 m/s - double dist_from_ball = collect::PARAM_stop_dist_scale * stopping_dist; - - Point target_pos = start.position() + dist_from_ball * (ball.position - start.position() + - velocity_scale * average_ball_vel_ * - non_zero_vel_time_delta) - .norm(); - LinearMotionInstant target{target_pos}; - - // save for is_done() - cached_robot_pos_ = start.position(); - cached_ball_pos_ = ball.position; - - // Try to use the RRTPathPlanner to generate the path first - // It reaches the target better for some reason - std::vector start_end_points{start.position(), target.position}; - - Replanner::PlanParams params{start, - target, - static_obstacles, - dynamic_obstacles, - plan_request.field_dimensions, - plan_request.constraints, - AngleFns::face_point(ball.position), - plan_request.shell_id}; - - Trajectory path = Replanner::create_plan(params, previous_); - - if (plan_request.debug_drawer != nullptr) { - plan_request.debug_drawer->draw_segment( - Segment(start.position(), start.position() + (target.position - start.position()) * 10), - QColor(255, 255, 255)); - } - - if (path.empty()) { - return Trajectory{}; - } - - path.set_debug_text("stopping"); - - // Make sure that when the path ends, we don't end up spinning around - // because we hit go past the ball position at the time of path creation - Point face_pt = start.position() + 10 * (target.position - start.position()).norm(); - - plan_angles(&path, start, AngleFns::face_point(face_pt), robot_constraints.rot); - - if (plan_request.debug_drawer != nullptr) { - plan_request.debug_drawer->draw_segment( - Segment(start.position(), - start.position() + Point::direction(AngleFns::face_point(face_pt)( - start.linear_motion(), start.heading(), nullptr)))); - } - - path.stamp(RJ::now()); - return path; -} - Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request, const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles) { @@ -679,32 +550,12 @@ void CollectPathPlanner::reset() { previous_ = Trajectory(); current_state_ = CollectPathPathPlannerStates::COARSE_APPROACH; average_ball_vel_initialized_ = false; - approach_direction_created_ = false; - control_path_created_ = false; path_coarse_target_initialized_ = false; + is_ball_sense_ = false; } bool CollectPathPlanner::is_done() const { - // FSM: CoarseApproach -> FineApproach -> Control - // (see process_state_transition()) - if (current_state_ != CONTROL) { - return false; - } else { - return true; - } - - // control state is done when the ball is slowed AND in mouth - // TODO(Kevin): make these into ROS planning params - double ball_slow_cutoff = 0.01; // m/s = ~10% robot radius/s - bool ball_is_slow = average_ball_vel_initialized_ && average_ball_vel_.mag() < ball_slow_cutoff; - - // ideal ball-mouth distance = 1 kRobotRadius + 1 kBallRadius - // give some leeway with ball_in_mouth_cutoff - double ball_in_mouth_cutoff = 0.01; - double dist_to_ball = (cached_robot_pos_.value() - cached_ball_pos_.value()).mag(); - bool ball_in_mouth = dist_to_ball - (kRobotRadius + kBallRadius) < ball_in_mouth_cutoff; - - return ball_is_slow && ball_in_mouth; + return is_ball_sense_; } } // namespace planning diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.hpp b/soccer/src/soccer/planning/planner/collect_path_planner.hpp index 7af6651def9..8da4d6e07bf 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.hpp @@ -24,8 +24,6 @@ class CollectPathPlanner : public PathPlanner { INTERCEPT, // From the slow part of the approach to the touching of the ball FINE_APPROACH, - // From touching the ball to stopped with the ball in the mouth - CONTROL }; CollectPathPlanner() @@ -58,11 +56,6 @@ class CollectPathPlanner : public PathPlanner { const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles); - Trajectory control(const PlanRequest& plan_request, RobotInstant start, - const Trajectory& partial_path, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles); - Trajectory invalid(const PlanRequest& plan_request, const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles); @@ -76,9 +69,6 @@ class CollectPathPlanner : public PathPlanner { bool average_ball_vel_initialized_ = false; rj_geometry::Point approach_direction_; - bool approach_direction_created_ = false; - - bool control_path_created_ = false; rj_geometry::Point path_coarse_target_; bool path_coarse_target_initialized_ = false; @@ -94,6 +84,7 @@ class CollectPathPlanner : public PathPlanner { std::optional cached_start_instant_; std::optional cached_robot_pos_; std::optional cached_ball_pos_; + bool is_ball_sense_ = false; // The direction to bounce the intercept to rj_geometry::Point target_bounce_direction_; diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 7f6899c6040..d0ee5350d45 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -90,7 +90,8 @@ Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) { delta_pos, face_pos); break; case SettlePathPlannerStates::Dampen: - result = dampen(plan_request, start_instant, delta_pos, face_pos); + result = dampen(plan_request, start_instant, static_obstacles, dynamic_obstacles, + delta_pos, face_pos); break; default: result = invalid(plan_request, static_obstacles, dynamic_obstacles); @@ -387,6 +388,8 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn } Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles, rj_geometry::Point delta_pos, rj_geometry::Point face_pos) { // Only run once if we can @@ -470,11 +473,15 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta Trajectory dampen_end; if (previous_.empty()) { - dampen_end = CreatePath::simple(start_instant.linear_motion(), final_stopping_motion, - plan_request.constraints.mot, start_instant.stamp); + dampen_end = CreatePath::intermediate(start_instant.linear_motion(), final_stopping_motion, + plan_request.constraints.mot, start_instant.stamp, + static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + plan_request.shell_id); } else { - dampen_end = CreatePath::simple(previous_.last().linear_motion(), final_stopping_motion, - plan_request.constraints.mot, previous_.last().stamp); + dampen_end = CreatePath::intermediate(previous_.last().linear_motion(), final_stopping_motion, + plan_request.constraints.mot, previous_.last().stamp, + static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + plan_request.shell_id); } dampen_end.set_debug_text("Damping"); diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.hpp b/soccer/src/soccer/planning/planner/settle_path_planner.hpp index 8d92625d865..db753dead03 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.hpp @@ -71,6 +71,8 @@ class SettlePathPlanner : public PathPlanner { // Dampen doesn't need to take obstacles into account. Trajectory dampen(const PlanRequest& plan_request, RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles, rj_geometry::Point delta_pos, rj_geometry::Point face_pos); Trajectory invalid(const PlanRequest& plan_request, diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 82adb2b338b..4078812e360 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -76,6 +76,12 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, target_angles.at(i) = angle_function(instant, target_angles.at(i - 1), &gradient); velocity.at(i) = trajectory->instant_at(i).linear_velocity().dot(rj_geometry::Point(gradient)); + if (std::isnan(velocity.at(i))) { + SPDLOG_INFO("SFSFSFS"); + SPDLOG_INFO("1: {}, {}", trajectory->instant_at(i).linear_velocity().x(), trajectory->instant_at(i).linear_velocity().y()); + SPDLOG_INFO("2: {}, {}", rj_geometry::Point(gradient).x(), rj_geometry::Point(gradient).y()); + + } } // TODO(#1506): Re-enable this. Currently the forward-tracking is disabled diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 3571e4b31f7..11d98c48ace 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -126,7 +126,7 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst if ((!trajectory_hits_static(trajectory, static_obstacles, start_time, nullptr))) { auto angle = (final_inter - start.position).angle(); cached_intermediate_tuple_[robot_id] = { - abs(angle), (final_inter - start.position).mag(), signbit(angle) ? -1 : 1}; + abs(angle), signbit(angle) ? -1 : 1, (final_inter - start.position).mag()}; return trajectory; } } diff --git a/soccer/src/soccer/planning/trajectory.cpp b/soccer/src/soccer/planning/trajectory.cpp index 1c9412e4c4e..0d2df9eb03c 100644 --- a/soccer/src/soccer/planning/trajectory.cpp +++ b/soccer/src/soccer/planning/trajectory.cpp @@ -23,7 +23,7 @@ Trajectory::Trajectory(Trajectory a, const Trajectory& b) { using rj_geometry::Point; if (!a_end.position().near_point(b_begin.position(), 1e-6) || - !a_end.linear_velocity().near_point(b_begin.linear_velocity, 1e-1) || + !a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-1) || a_end.stamp != b_begin.stamp) { SPDLOG_ERROR("points near? {}, vels near? {}, timestamps match? {}", a_end.position().near_point(b_begin.position(), 1e-6), diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index f4b9a3af148..3daf2fb1bc3 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -294,8 +294,7 @@ std::optional Offense::state_to_task(RobotIntent intent) { // intent.motion_command = settle_cmd; // intent.dribbler_speed = 255.0; // } else { - auto collect_cmd = planning::MotionCommand{"settle"}; - collect_cmd.target = planning::LinearMotionInstant{last_world_state_->ball.position}; + auto collect_cmd = planning::MotionCommand{"collect"}; intent.motion_command = collect_cmd; intent.dribbler_speed = 255.0; // } From d308597441ba62fbbb51dc1a0b56e859bae4bbad Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 16 Feb 2025 20:21:52 -0500 Subject: [PATCH 09/12] completed settle into colelct merge --- .../planning/planner/collect_path_planner.cpp | 160 +++++++++++++++++- .../planning/planner/collect_path_planner.hpp | 24 ++- .../planning/planner/settle_path_planner.cpp | 1 + 3 files changed, 172 insertions(+), 13 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 841c13fd36e..271814d6d12 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -99,7 +99,7 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { } // Process the state transitions - process_state_transition(plan_request, ball, start_instant); + process_state_transition(plan_request, ball, &start_instant); // List of obstacles ShapeSet static_obstacles; @@ -136,6 +136,11 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles); break; } + // Dampen a moving ball + case DAMPEN: { + previous_ = dampen(plan_request, start_instant, static_obstacles, dynamic_obstacles); + break; + } default: previous_ = invalid(plan_request, static_obstacles, dynamic_obstacles); break; @@ -145,7 +150,7 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { } void CollectPathPlanner::process_state_transition(const PlanRequest& request, BallState ball, - RobotInstant start_instant) { + RobotInstant* start_instant) { // If the ball is moving, intercept // if not, regularly approach if (current_state_ == COARSE_APPROACH && average_ball_vel_.mag() > kInterceptVelocityThreshold) { @@ -155,13 +160,13 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba } // Do the transitions - double dist = (start_instant.position() - ball.position).mag() - kRobotMouthRadius; - double speed_diff = (start_instant.linear_velocity() - average_ball_vel_).mag() - + double dist = (start_instant->position() - ball.position).mag() - kRobotMouthRadius; + double speed_diff = (start_instant->linear_velocity() - average_ball_vel_).mag() - collect::PARAM_touch_delta_speed; // If we are in range to the slow dist if (dist < collect::PARAM_approach_dist_target + kRobotMouthRadius && - (current_state_ == COARSE_APPROACH || current_state_ == INTERCEPT)) { + (current_state_ == COARSE_APPROACH)) { current_state_ = FINE_APPROACH; } @@ -171,6 +176,39 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba current_state_ = COARSE_APPROACH; } + // Intercept -> Dampen, PrevPath and almost at the end of the path + if (!previous_.empty() && start_instant->stamp > previous_.begin_time() && + start_instant->stamp <= previous_.end_time()) { + rj_geometry::Line ball_movement_line(ball.position, ball.position + average_ball_vel_); + + Trajectory path_so_far = + previous_.sub_trajectory(previous_.begin_time(), start_instant->stamp); + double bot_dist_to_ball_movement_line = + ball_movement_line.dist_to(path_so_far.last().position()); + + // Intercept -> Dampen + // Almost intersecting the ball path and + // Almost at end of the target path or + // Already in line with the ball + // Within X seconds of the end of path + bool inline_with_ball = bot_dist_to_ball_movement_line < kRobotMouthRadius / 2; + bool in_front_of_ball = + average_ball_vel_.angle_between(start_instant->position() - ball.position) < 3.14 / 2; + + if (in_front_of_ball && inline_with_ball && + current_state_ == INTERCEPT) { + // Start the next section of the path from the end of our current + // path + *start_instant = path_so_far.last(); + current_state_ = DAMPEN; + } + } + + // Dampen -> Fine Approach if ball is sufficiently slow + if (average_ball_vel_.mag() < kDampenBallSpeedThreshold && current_state_ == DAMPEN) { + current_state_ = FINE_APPROACH; + } + // If we are in FineApproach and we have the ball, terminate is_ball_sense_ = request.ball_sense && current_state_ == FINE_APPROACH; } @@ -428,7 +466,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, } if (start_instant.position().dist_to(ball.position) < start_instant.position().dist_to(path_intercept_target_) && - abs(average_ball_vel_.angle_to(start_instant.position() - ball.position)) < degrees_to_radians(60)) { + average_ball_vel_.angle_between(ball.position - start_instant.position()) < degrees_to_radians(kChaseAngleThreshold)) { path_intercept_target_ = ball.position; } @@ -461,6 +499,116 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, return new_target_path; } +Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles) { + // Only run once if we can + + // Intercept ends with a % ball velocity in the direction of the ball + // movement Slow down once ball is nearby to 0 m/s + + // Try to slow down as fast as possible to 0 m/s along the ball path + // We have to do position control since we want to stay in the line of the + // ball while we do this. If we did velocity, we have very little control of + // where on the field it is without some other position controller. + + // Uses constant acceleration to create a linear velocity profile + + // TODO(Kyle): Realize the ball will probably bounce off the robot + // so we can use that vector to stop + // Save vector and use that? + BallState ball = plan_request.world_state->ball; + + rj_geometry::Point face_pos = start_instant.position() + Point::direction((ball.position - start_instant.position()).angle()) * 10; + + if (plan_request.debug_drawer != nullptr) { + plan_request.debug_drawer->draw_text("Damping", ball.position + Point(.1, .1), + QColor(255, 255, 255)); + } + + if (path_created_for_dampen_ && !previous_.empty()) { + return previous_; + } + + path_created_for_dampen_ = true; + + if (!previous_.empty()) { + start_instant = previous_.last(); + } + + // Using the current velocity + // Calculate stopping point along the ball path + double max_accel = plan_request.constraints.mot.max_acceleration; + double current_speed = start_instant.linear_velocity().mag(); + + // Assuming const accel going to zero velocity + // speed / accel gives time to stop + // speed / 2 is average time over the entire operation + double stopping_dist = current_speed * current_speed / (2 * max_accel); + + // Offset entire ball line to just be the line we want the robot + // to move down + // Accounts for weird targets + Point ball_movement_dir(average_ball_vel_.normalized()); + Line ball_movement_line(ball.position, + ball.position + ball_movement_dir); + Point nearest_point_to_robot = ball_movement_line.nearest_point(start_instant.position()); + double dist_to_ball_movement_line = (start_instant.position() - nearest_point_to_robot).mag(); + + // Default to just moving to the closest point on the line + Point final_stopping_point(nearest_point_to_robot); + + // Make sure we are actually moving before we start trying to optimize stuff + if (stopping_dist >= 0.01f) { + // The closer we are to the line, the less we should move into the line + // to stop overshoot + double percent_stopping_dist_to_ball_movement_line = + dist_to_ball_movement_line / stopping_dist; + + // 0% should be just stopping at stopping_dist down the ball movement + // line from the nearest_point_to_robot 100% or more should just be trying + // to get to the nearest_point_to_robot (Default case) + if (percent_stopping_dist_to_ball_movement_line < 1) { + // c^2 - a^2 = b^2 + // c is stopping dist, a is dist to ball line + // b is dist down ball line + double dist_down_ball_movement_line = + std::sqrt(stopping_dist * stopping_dist - + dist_to_ball_movement_line * dist_to_ball_movement_line); + final_stopping_point = + nearest_point_to_robot + dist_down_ball_movement_line * ball_movement_dir; + } + } + + // Target stopping point with 0 speed. + LinearMotionInstant final_stopping_motion{final_stopping_point}; + + Trajectory dampen_end; + + if (previous_.empty()) { + dampen_end = CreatePath::intermediate(start_instant.linear_motion(), final_stopping_motion, + plan_request.constraints.mot, start_instant.stamp, + static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + plan_request.shell_id); + } else { + dampen_end = CreatePath::intermediate(previous_.last().linear_motion(), final_stopping_motion, + plan_request.constraints.mot, previous_.last().stamp, + static_obstacles, dynamic_obstacles, plan_request.field_dimensions, + plan_request.shell_id); + } + + dampen_end.set_debug_text("Damping"); + + if (!previous_.empty()) { + dampen_end = Trajectory(previous_, dampen_end); + } + + plan_angles(&dampen_end, start_instant, AngleFns::face_point(face_pos), + plan_request.constraints.rot); + dampen_end.stamp(RJ::now()); + return dampen_end; +} + Trajectory CollectPathPlanner::fine_approach( const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.hpp b/soccer/src/soccer/planning/planner/collect_path_planner.hpp index 8da4d6e07bf..62d4bb15776 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.hpp @@ -22,6 +22,8 @@ class CollectPathPlanner : public PathPlanner { COARSE_APPROACH, // Intercepts a moving ball INTERCEPT, + // Slows down the velocity of an intercepted ball + DAMPEN, // From the slow part of the approach to the touching of the ball FINE_APPROACH, }; @@ -40,7 +42,7 @@ class CollectPathPlanner : public PathPlanner { void check_solution_validity(BallState ball, RobotInstant start); void process_state_transition(const PlanRequest& request, BallState ball, - RobotInstant start_instant); + RobotInstant* start_instant); Trajectory coarse_approach( const PlanRequest& plan_request, RobotInstant start, @@ -51,6 +53,11 @@ class CollectPathPlanner : public PathPlanner { const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles); + // Dampen doesn't need to take obstacles into account. + Trajectory dampen(const PlanRequest& plan_request, RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles); + Trajectory fine_approach( const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, @@ -79,18 +86,21 @@ class CollectPathPlanner : public PathPlanner { // Only change the target of the path if it changes significantly rj_geometry::Point path_intercept_target_; + + // Have we already made a dampen path + bool path_created_for_dampen_ = false; - // is_done vars - std::optional cached_start_instant_; - std::optional cached_robot_pos_; - std::optional cached_ball_pos_; + // Do we have the ball in the robot bool is_ball_sense_ = false; - // The direction to bounce the intercept to - rj_geometry::Point target_bounce_direction_; + // Threshold for switching from dampen to fine approach + double kDampenBallSpeedThreshold = 0.75; // Threshold for ball velocity to try to intercept; double kInterceptVelocityThreshold = 0.2; + + // Threshold for chasing after the ball instead of intercepting (deg) + double kChaseAngleThreshold = 45; }; } // namespace planning diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index d0ee5350d45..9708dcae734 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -18,6 +18,7 @@ using namespace rj_geometry; namespace planning { +// Do not use anymore, use collect Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) { const auto state = plan_request.play_state.state(); if (state == PlayState::Stop || state == PlayState::Halt) { From 319ec4e87c0434e30a49e2b28b3693364f2b75f1 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 16 Feb 2025 20:25:09 -0500 Subject: [PATCH 10/12] removed angle planning NaN --- soccer/src/soccer/planning/primitives/angle_planning.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 4078812e360..82adb2b338b 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -76,12 +76,6 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, target_angles.at(i) = angle_function(instant, target_angles.at(i - 1), &gradient); velocity.at(i) = trajectory->instant_at(i).linear_velocity().dot(rj_geometry::Point(gradient)); - if (std::isnan(velocity.at(i))) { - SPDLOG_INFO("SFSFSFS"); - SPDLOG_INFO("1: {}, {}", trajectory->instant_at(i).linear_velocity().x(), trajectory->instant_at(i).linear_velocity().y()); - SPDLOG_INFO("2: {}, {}", rj_geometry::Point(gradient).x(), rj_geometry::Point(gradient).y()); - - } } // TODO(#1506): Re-enable this. Currently the forward-tracking is disabled From 89483b5ffad9a5b537a94efd7088280565f6aa2e Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 16 Feb 2025 20:27:13 -0500 Subject: [PATCH 11/12] Fix Code Style On settle-collect (#2347) automated style fixes Co-authored-by: sid-parikh --- .../planning/planner/collect_path_planner.cpp | 38 +++++++++---------- .../planning/planner/collect_path_planner.hpp | 2 +- .../planning/planner/settle_path_planner.cpp | 16 ++++---- .../planning/primitives/create_path.cpp | 4 +- soccer/src/soccer/planning/trajectory.cpp | 2 +- 5 files changed, 31 insertions(+), 31 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 2a87191ad2d..9ec61bb9ffd 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -197,8 +197,7 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba bool in_front_of_ball = average_ball_vel_.angle_between(start_instant->position() - ball.position) < 3.14 / 2; - if (in_front_of_ball && inline_with_ball && - current_state_ == INTERCEPT) { + if (in_front_of_ball && inline_with_ball && current_state_ == INTERCEPT) { // Start the next section of the path from the end of our current // path *start_instant = path_so_far.last(); @@ -469,8 +468,10 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, path_intercept_target_ = avg_instantaneous_intercept_target_; } - if (start_instant.position().dist_to(ball.position) < start_instant.position().dist_to(path_intercept_target_) && - average_ball_vel_.angle_between(ball.position - start_instant.position()) < degrees_to_radians(kChaseAngleThreshold)) { + if (start_instant.position().dist_to(ball.position) < + start_instant.position().dist_to(path_intercept_target_) && + average_ball_vel_.angle_between(ball.position - start_instant.position()) < + degrees_to_radians(kChaseAngleThreshold)) { path_intercept_target_ = ball.position; } @@ -504,8 +505,8 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, } Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInstant start_instant, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles) { + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles) { // Only run once if we can // Intercept ends with a % ball velocity in the direction of the ball @@ -523,7 +524,9 @@ Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInst // Save vector and use that? BallState ball = plan_request.world_state->ball; - rj_geometry::Point face_pos = start_instant.position() + Point::direction((ball.position - start_instant.position()).angle()) * 10; + rj_geometry::Point face_pos = + start_instant.position() + + Point::direction((ball.position - start_instant.position()).angle()) * 10; if (plan_request.debug_drawer != nullptr) { plan_request.debug_drawer->draw_text("Damping", ball.position + Point(.1, .1), @@ -554,8 +557,7 @@ Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInst // to move down // Accounts for weird targets Point ball_movement_dir(average_ball_vel_.normalized()); - Line ball_movement_line(ball.position, - ball.position + ball_movement_dir); + Line ball_movement_line(ball.position, ball.position + ball_movement_dir); Point nearest_point_to_robot = ball_movement_line.nearest_point(start_instant.position()); double dist_to_ball_movement_line = (start_instant.position() - nearest_point_to_robot).mag(); @@ -591,14 +593,14 @@ Trajectory CollectPathPlanner::dampen(const PlanRequest& plan_request, RobotInst if (previous_.empty()) { dampen_end = CreatePath::intermediate(start_instant.linear_motion(), final_stopping_motion, - plan_request.constraints.mot, start_instant.stamp, - static_obstacles, dynamic_obstacles, plan_request.field_dimensions, - plan_request.shell_id); + plan_request.constraints.mot, start_instant.stamp, + static_obstacles, dynamic_obstacles, + plan_request.field_dimensions, plan_request.shell_id); } else { - dampen_end = CreatePath::intermediate(previous_.last().linear_motion(), final_stopping_motion, - plan_request.constraints.mot, previous_.last().stamp, - static_obstacles, dynamic_obstacles, plan_request.field_dimensions, - plan_request.shell_id); + dampen_end = CreatePath::intermediate( + previous_.last().linear_motion(), final_stopping_motion, plan_request.constraints.mot, + previous_.last().stamp, static_obstacles, dynamic_obstacles, + plan_request.field_dimensions, plan_request.shell_id); } dampen_end.set_debug_text("Damping"); @@ -706,8 +708,6 @@ void CollectPathPlanner::reset() { is_ball_sense_ = false; } -bool CollectPathPlanner::is_done() const { - return is_ball_sense_; -} +bool CollectPathPlanner::is_done() const { return is_ball_sense_; } } // namespace planning diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.hpp b/soccer/src/soccer/planning/planner/collect_path_planner.hpp index 62d4bb15776..2ef806c15f8 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.hpp @@ -86,7 +86,7 @@ class CollectPathPlanner : public PathPlanner { // Only change the target of the path if it changes significantly rj_geometry::Point path_intercept_target_; - + // Have we already made a dampen path bool path_created_for_dampen_ = false; diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 9708dcae734..5278a426305 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -91,7 +91,7 @@ Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) { delta_pos, face_pos); break; case SettlePathPlannerStates::Dampen: - result = dampen(plan_request, start_instant, static_obstacles, dynamic_obstacles, + result = dampen(plan_request, start_instant, static_obstacles, dynamic_obstacles, delta_pos, face_pos); break; default: @@ -475,14 +475,14 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta if (previous_.empty()) { dampen_end = CreatePath::intermediate(start_instant.linear_motion(), final_stopping_motion, - plan_request.constraints.mot, start_instant.stamp, - static_obstacles, dynamic_obstacles, plan_request.field_dimensions, - plan_request.shell_id); + plan_request.constraints.mot, start_instant.stamp, + static_obstacles, dynamic_obstacles, + plan_request.field_dimensions, plan_request.shell_id); } else { - dampen_end = CreatePath::intermediate(previous_.last().linear_motion(), final_stopping_motion, - plan_request.constraints.mot, previous_.last().stamp, - static_obstacles, dynamic_obstacles, plan_request.field_dimensions, - plan_request.shell_id); + dampen_end = CreatePath::intermediate( + previous_.last().linear_motion(), final_stopping_motion, plan_request.constraints.mot, + previous_.last().stamp, static_obstacles, dynamic_obstacles, + plan_request.field_dimensions, plan_request.shell_id); } dampen_end.set_debug_text("Damping"); diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 11d98c48ace..24a5becb775 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -125,8 +125,8 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst // If the trajectory does not hit an obstacle, it is valid if ((!trajectory_hits_static(trajectory, static_obstacles, start_time, nullptr))) { auto angle = (final_inter - start.position).angle(); - cached_intermediate_tuple_[robot_id] = { - abs(angle), signbit(angle) ? -1 : 1, (final_inter - start.position).mag()}; + cached_intermediate_tuple_[robot_id] = {abs(angle), signbit(angle) ? -1 : 1, + (final_inter - start.position).mag()}; return trajectory; } } diff --git a/soccer/src/soccer/planning/trajectory.cpp b/soccer/src/soccer/planning/trajectory.cpp index 0d2df9eb03c..0af6fec5c57 100644 --- a/soccer/src/soccer/planning/trajectory.cpp +++ b/soccer/src/soccer/planning/trajectory.cpp @@ -23,7 +23,7 @@ Trajectory::Trajectory(Trajectory a, const Trajectory& b) { using rj_geometry::Point; if (!a_end.position().near_point(b_begin.position(), 1e-6) || - !a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-1) || + !a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-1) || a_end.stamp != b_begin.stamp) { SPDLOG_ERROR("points near? {}, vels near? {}, timestamps match? {}", a_end.position().near_point(b_begin.position(), 1e-6), From 25928153f78c364cb22765ef8d23310dd9c27ff9 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 16 Feb 2025 21:49:38 -0500 Subject: [PATCH 12/12] fix naming --- .../planning/planner/collect_path_planner.cpp | 13 ++----------- .../planning/planner/collect_path_planner.hpp | 6 +++--- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 9ec61bb9ffd..d6a5aa8346a 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -104,14 +104,7 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { // List of obstacles ShapeSet static_obstacles; std::vector dynamic_obstacles; - - // If we are intercepting, do add the ball as an obstacle - if (current_state_ == INTERCEPT) { - fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, true); - } else { - Trajectory ball; - fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false, &ball); - } + fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false); // Return an empty trajectory if the ball is hitting static obstacles // or it is in the goalie area. @@ -163,8 +156,6 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba // Do the transitions double dist = (start_instant->position() - ball.position).mag() - kRobotMouthRadius; - double speed_diff = (start_instant->linear_velocity() - average_ball_vel_).mag() - - collect::PARAM_touch_delta_speed; // If we are in range to the slow dist if (dist < collect::PARAM_approach_dist_target + kRobotMouthRadius && @@ -195,7 +186,7 @@ void CollectPathPlanner::process_state_transition(const PlanRequest& request, Ba // Within X seconds of the end of path bool inline_with_ball = bot_dist_to_ball_movement_line < kRobotMouthRadius / 2; bool in_front_of_ball = - average_ball_vel_.angle_between(start_instant->position() - ball.position) < 3.14 / 2; + average_ball_vel_.angle_between(start_instant->position() - ball.position) < M_PI / 2; if (in_front_of_ball && inline_with_ball && current_state_ == INTERCEPT) { // Start the next section of the path from the end of our current diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.hpp b/soccer/src/soccer/planning/planner/collect_path_planner.hpp index 2ef806c15f8..c6b83387213 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.hpp @@ -94,13 +94,13 @@ class CollectPathPlanner : public PathPlanner { bool is_ball_sense_ = false; // Threshold for switching from dampen to fine approach - double kDampenBallSpeedThreshold = 0.75; + static constexpr double kDampenBallSpeedThreshold{0.75}; // Threshold for ball velocity to try to intercept; - double kInterceptVelocityThreshold = 0.2; + static constexpr double kInterceptVelocityThreshold{0.2}; // Threshold for chasing after the ball instead of intercepting (deg) - double kChaseAngleThreshold = 45; + static constexpr double kChaseAngleThreshold{45}; }; } // namespace planning