From aea314c9b42de2cdc7c8bd4bc8ac339d9a36a93d Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 29 Sep 2024 19:51:30 -0400 Subject: [PATCH] Intermediate Pathgen (#2266) * initial test of tigers system * clean up and add comment * added line test * fix line issue * made comments and removed from settle * Fix Code Style On intermediate-pathgen (#2267) automated style fixes Co-authored-by: sanatd33 * make pr changes * add cache inter * start cacching intermediates * add params * use abs angle * Fix Code Style On intermediate-pathgen (#2269) automated style fixes Co-authored-by: sid-parikh --------- Co-authored-by: rishiso Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: sanatd33 Co-authored-by: petergarud Co-authored-by: sid-parikh --- soccer/src/soccer/CMakeLists.txt | 1 + .../planner/line_kick_path_planner.cpp | 2 + .../planning/planner/settle_path_planner.cpp | 12 +- .../src/soccer/planning/planning_params.cpp | 13 +++ .../src/soccer/planning/planning_params.hpp | 7 ++ .../planning/primitives/create_path.cpp | 101 +++++++++++++++++ .../planning/primitives/create_path.hpp | 11 +- .../soccer/planning/primitives/replanner.cpp | 17 ++- .../soccer/strategy/agent/position/line.cpp | 105 ++++++++++++++++++ .../soccer/strategy/agent/position/line.hpp | 25 +++++ .../agent/position/robot_factory_position.hpp | 7 +- 11 files changed, 282 insertions(+), 19 deletions(-) create mode 100644 soccer/src/soccer/strategy/agent/position/line.cpp create mode 100644 soccer/src/soccer/strategy/agent/position/line.hpp diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 6412fb91829..ad698cf41f5 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -79,6 +79,7 @@ set(ROBOCUP_LIB_SRC strategy/agent/position/position.cpp strategy/agent/position/robot_factory_position.cpp strategy/agent/position/goalie.cpp + strategy/agent/position/line.cpp strategy/agent/position/offense.cpp strategy/agent/position/idle.cpp strategy/agent/position/defense.cpp diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp index ecbf3732a47..1faf9dbb467 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -88,6 +88,8 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { MotionCommand modified_command{"path_target", target, FacePoint{plan_request.motion_command.target.position}}; + + modified_command.ignore_ball = true; modified_request.motion_command = modified_command; return path_target_.plan(modified_request); diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index dde32594b56..6e4cf03258f 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -221,9 +221,9 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn // Plan a path from our partial path start location to the intercept // test location - Trajectory path = CreatePath::rrt(start_instant.linear_motion(), target_robot_intersection, - plan_request.constraints.mot, start_instant.stamp, - static_obstacles, dynamic_obstacles); + Trajectory path = CreatePath::intermediate( + start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot, + start_instant.stamp, static_obstacles); // Calculate the RJ::Seconds buffer_duration = ball_time - path.duration(); @@ -332,9 +332,9 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn LinearMotionInstant target{closest_pt, settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_}; - Trajectory shortcut = - CreatePath::rrt(start_instant.linear_motion(), target, plan_request.constraints.mot, - start_instant.stamp, static_obstacles, dynamic_obstacles); + Trajectory shortcut = CreatePath::intermediate(start_instant.linear_motion(), target, + plan_request.constraints.mot, + start_instant.stamp, static_obstacles); if (!shortcut.empty()) { plan_angles(&shortcut, start_instant, AngleFns::face_point(face_pos), diff --git a/soccer/src/soccer/planning/planning_params.cpp b/soccer/src/soccer/planning/planning_params.cpp index 6a6d3621fed..e94b3abba75 100644 --- a/soccer/src/soccer/planning/planning_params.cpp +++ b/soccer/src/soccer/planning/planning_params.cpp @@ -41,6 +41,19 @@ DEFINE_NS_INT64(kPlanningParamModule, rrt, min_iterations, 50, DEFINE_NS_INT64(kPlanningParamModule, rrt, max_iterations, 500, "Maximum number of RRT iterations to run before giving up"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale, 0.5, + "Minimum length for intermediate point (m)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale, 1.5, + "Maximum length for intermediate point (m)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle, 20, + "Minimum angle for intermediate point (deg)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle, 140, + "Maximum angle for intermediate point (deg)"); +DEFINE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates, 5, + "Number of intermediate points used (unitless)"); +DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size, 0.1, + "Step size for testing intermediates (m)"); + DEFINE_NS_FLOAT64( kPlanningParamModule, escape, step_size, 0.1, "Step size for the RRT used to find an unblocked point in find_non_blocked_goal()"); diff --git a/soccer/src/soccer/planning/planning_params.hpp b/soccer/src/soccer/planning/planning_params.hpp index 03a88917244..f22fe373969 100644 --- a/soccer/src/soccer/planning/planning_params.hpp +++ b/soccer/src/soccer/planning/planning_params.hpp @@ -25,6 +25,13 @@ DECLARE_NS_FLOAT64(kPlanningParamModule, rrt, waypoint_bias); DECLARE_NS_INT64(kPlanningParamModule, rrt, min_iterations); DECLARE_NS_INT64(kPlanningParamModule, rrt, max_iterations); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle); +DECLARE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates); +DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size); + DECLARE_NS_FLOAT64(kPlanningParamModule, escape, step_size); DECLARE_NS_FLOAT64(kPlanningParamModule, escape, goal_change_threshold); diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 11feff152d7..d510483a0ad 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -75,4 +75,105 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal return path; } +static std::optional> cached_intermediate_tuple_{}; + +Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, + const MotionConstraints& motion_constraints, RJ::Time start_time, + const rj_geometry::ShapeSet& static_obstacles) { + // if already on goal, no need to move + if (start.position.dist_to(goal.position) < 1e-6) { + return Trajectory{{RobotInstant{Pose(start.position, 0), Twist(), start_time}}}; + } + + // maybe straight line works + Trajectory straight_trajectory = + CreatePath::simple(start, goal, motion_constraints, start_time); + + // If we are very close to the goal (i.e. there physically can't be a robot + // in our way) or the straight trajectory is feasible, we can use it. + if (start.position.dist_to(goal.position) < kRobotRadius || + (!trajectory_hits_static(straight_trajectory, static_obstacles, start_time, nullptr))) { + return straight_trajectory; + } + + // Generate list of intermediate points + std::vector intermediates = get_intermediates(start, goal); + + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { + rj_geometry::Point final_inter = intermediates[i]; + + // Step through the path from the robot to the final intermediate point + // and test each point on that path as an intermediate point + for (double t = intermediate::PARAM_step_size; t < final_inter.dist_to(start.position); + t += intermediate::PARAM_step_size) { + rj_geometry::Point intermediate = + (final_inter - start.position).normalized(t) + start.position; + Trajectory trajectory = + CreatePath::simple(start, goal, motion_constraints, start_time, {intermediate}); + + // 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_ = {abs(angle), signbit(angle) ? -1 : 1, + (final_inter - start.position).mag()}; + return trajectory; + } + } + } + + // If all else fails, return the straight-line trajectory + return straight_trajectory; +} + +std::vector get_intermediates(const LinearMotionInstant& start, + const LinearMotionInstant& goal) { + std::random_device rd; + std::mt19937 gen(rd()); + // Create a random distribution for the distance between the start + // and the intermediate points + std::uniform_real_distribution<> scale_dist(intermediate::PARAM_min_scale, + intermediate::PARAM_max_scale); + + double angle_range = intermediate::PARAM_max_angle - intermediate::PARAM_min_angle; + // Create a random distribution for the angle between the start + // and the intermediate points + std::uniform_real_distribution<> angle_dist(-angle_range, angle_range); + + std::vector intermediates; + std::vector> inter_tuples; + + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { + double angle = angle_dist(gen); + angle += std::copysign(intermediate::PARAM_min_angle, angle); + angle = degrees_to_radians(angle); + double scale = scale_dist(gen); + + // Generate random tuples of distances and angles + inter_tuples.emplace_back(abs(angle), signbit(angle) ? -1 : 1, scale); + } + + if (cached_intermediate_tuple_) { + inter_tuples.push_back(*cached_intermediate_tuple_); + } + + // Sort the list of tuples by the magnitude of angle + // This ensures that we take paths with + // smaller offsets from the simple path + sort(inter_tuples.begin(), inter_tuples.end()); + + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { + double angle = std::get<0>(inter_tuples[i]) * std::get<1>(inter_tuples[i]); + double scale = std::get<2>(inter_tuples[i]); + + double fin_angle = goal.position.angle_to(start.position) + angle; + double fin_length = scale; + + // Convert the distances and angles into a point + intermediates.push_back(start.position + rj_geometry::Point{fin_length * cos(fin_angle), + fin_length * sin(fin_angle)}); + } + + return intermediates; +} + } // namespace planning::CreatePath diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index ad73bcdc8b7..d71cf5809f8 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -1,8 +1,11 @@ #pragma once +#include +#include + #include "planning/motion_constraints.hpp" -#include "planning/trajectory.hpp" #include "planning/primitives/path_smoothing.hpp" +#include "planning/trajectory.hpp" namespace planning::CreatePath { @@ -24,4 +27,10 @@ Trajectory simple( const MotionConstraints& motion_constraints, RJ::Time start_time, const std::vector& intermediate_points = {}); +Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, + const MotionConstraints& motion_constraints, RJ::Time start_time, + const rj_geometry::ShapeSet& static_obstacles); + +std::vector get_intermediates(const LinearMotionInstant& start, + const LinearMotionInstant& goal); } // namespace planning::CreatePath \ No newline at end of file diff --git a/soccer/src/soccer/planning/primitives/replanner.cpp b/soccer/src/soccer/planning/primitives/replanner.cpp index 69da4a2e407..3c5725f5047 100644 --- a/soccer/src/soccer/planning/primitives/replanner.cpp +++ b/soccer/src/soccer/planning/primitives/replanner.cpp @@ -31,10 +31,9 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& } Trajectory pre_trajectory = partial_path(previous, params.start.stamp); - Trajectory post_trajectory = - CreatePath::rrt(pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot, - pre_trajectory.end_time(), params.static_obstacles, - params.dynamic_obstacles, bias_waypoints); + Trajectory post_trajectory = CreatePath::intermediate( + pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot, + pre_trajectory.end_time(), params.static_obstacles); // If we couldn't profile such that velocity at the end of the partial replan period is valid, // do a full replan. @@ -57,8 +56,8 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { Trajectory path = - CreatePath::rrt(params.start.linear_motion(), params.goal, params.constraints.mot, - params.start.stamp, params.static_obstacles, params.dynamic_obstacles); + CreatePath::intermediate(params.start.linear_motion(), params.goal, params.constraints.mot, + params.start.stamp, params.static_obstacles); // if the initial path is empty, the goal must be blocked // try to shift the goal_point until it is no longer blocked @@ -76,9 +75,9 @@ Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { almost_goal.position += shift_dir * shift_size; - path = - CreatePath::rrt(params.start.linear_motion(), almost_goal, params.constraints.mot, - params.start.stamp, params.static_obstacles, params.dynamic_obstacles); + path = CreatePath::intermediate(params.start.linear_motion(), almost_goal, + params.constraints.mot, params.start.stamp, + params.static_obstacles); } if (!path.empty()) { diff --git a/soccer/src/soccer/strategy/agent/position/line.cpp b/soccer/src/soccer/strategy/agent/position/line.cpp new file mode 100644 index 00000000000..daf99ef1112 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/line.cpp @@ -0,0 +1,105 @@ +#include "line.hpp" + +namespace strategy { + +Line::Line(const Position& other) : Position{other} { position_name_ = "Line"; } + +Line::Line(int r_id) : Position{r_id, "Line"} {} + +std::optional Line::derived_get_task(RobotIntent intent) { + if (check_is_done()) { + forward_ = !forward_; + } + + if (vertical_) { + if (forward_) { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.center_field_loc().x() - (robot_id_ - 3) * 1, + field_dimensions_.center_field_loc().y() - 2.5 + 5 * 0.75, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceTarget(), true}; + + intent.motion_command = motion_command; + } else { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.center_field_loc().x() - (robot_id_ - 3) * 1, + field_dimensions_.center_field_loc().y() - 4.5 + 5 * 0.75, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceTarget(), true}; + + intent.motion_command = motion_command; + } + } else { + if (forward_) { + if (face_target_) { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.center_field_loc().x() - 2.5 + 5 * 0.75, + (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceTarget(), true}; + + intent.motion_command = motion_command; + } else { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.our_defense_area().maxx(), + (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceAngle{0}, true}; + + intent.motion_command = motion_command; + } + + } else { + if (face_target_) { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.our_defense_area().minx(), + (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceTarget(), true}; + + intent.motion_command = motion_command; + } else { + auto motion_command = planning::MotionCommand{ + "path_target", + planning::LinearMotionInstant{ + rj_geometry::Point{ + field_dimensions_.our_defense_area().minx(), + (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1, + }, + rj_geometry::Point{0.0, 0.0}, + }, + planning::FaceAngle{0}, true}; + + intent.motion_command = motion_command; + } + } + } + + return intent; +} +} // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/line.hpp b/soccer/src/soccer/strategy/agent/position/line.hpp new file mode 100644 index 00000000000..a47b31701a6 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/line.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include + +#include "position.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace strategy { +class Line : public Position { +public: + Line(const Position& other); + Line(int r_id); + ~Line() override = default; + Line(const Line& other) = default; + Line(Line&& other) = default; + + std::string get_current_state() override { return "Line"; } + +private: + std::optional derived_get_task(RobotIntent intent) override; + bool forward_ = true; + bool vertical_ = false; + bool face_target_ = false; +}; +} // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp index 3df719c41fe..67f4cbfa1e1 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -20,6 +20,7 @@ #include "strategy/agent/position/free_kicker.hpp" #include "strategy/agent/position/goal_kicker.hpp" #include "strategy/agent/position/goalie.hpp" +#include "strategy/agent/position/line.hpp" #include "strategy/agent/position/offense.hpp" #include "strategy/agent/position/penalty_non_kicker.hpp" #include "strategy/agent/position/penalty_player.hpp" @@ -147,9 +148,9 @@ class RobotFactoryPosition : public Position { // return; // } if (dynamic_cast(current_position_.get()) == nullptr) { - // SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name()); - // This line requires Pos to implement the constructor Pos(const - // Position&) + // This line requires Pos to implement the constructor Pos(const + // Position&) + SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name()); current_position_->die(); current_position_ = std::make_unique(*current_position_); }