From 595b1e110533615c236d5e9642b9e2d9eaadb952 Mon Sep 17 00:00:00 2001 From: rishiso Date: Tue, 27 Aug 2024 21:48:12 -0400 Subject: [PATCH 01/12] initial test of tigers system --- .../planner/line_kick_path_planner.cpp | 2 + .../planning/primitives/create_path.cpp | 82 +++++++++++++++++++ .../planning/primitives/create_path.hpp | 18 ++++ .../soccer/planning/primitives/replanner.cpp | 13 ++- 4 files changed, 108 insertions(+), 7 deletions(-) diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp index 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/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 11feff152d7..0535dc80fe5 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -75,4 +75,86 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal return path; } +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 we don't need an RRT + 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; + } + + + std::vector intermediates = get_intermediates(start, goal); + + + for (int i = 0; i < NUM_INTERMEDIATES; i++) { + rj_geometry::Point final_inter = intermediates[i]; + + for (double t = STEP_SIZE; t < final_inter.dist_to(start.position); t += 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}); + + RJ::Time first_hit_time = RJ::Time::max(), second_hit_time = RJ::Time::max();; + bool first_hits = trajectory_hits_static(trajectory, static_obstacles, start_time, &first_hit_time); + + if ((!first_hits)) { + return trajectory; + } + } + } + + return straight_trajectory; +} + + +std::vector get_intermediates( + const LinearMotionInstant& start, + const LinearMotionInstant& goal) { + + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> scale_dist(MIN_SCALE, MAX_SCALE); + double angle_range = MAX_ANGLE - MIN_ANGLE; + std::uniform_real_distribution<> angle_dist(-angle_range, angle_range); + + std::vector intermediates; + std::vector> inter_pairs; + + for (int i = 0; i < NUM_INTERMEDIATES; i++) { + double angle = angle_dist(gen); + angle += std::copysign(MIN_ANGLE, angle); + angle = degrees_to_radians(angle); + double scale = scale_dist(gen); + + inter_pairs.emplace_back(angle, scale); + } + sort(inter_pairs.begin(), inter_pairs.end()); + + for (int i = 0; i < NUM_INTERMEDIATES; i++) { + double angle = inter_pairs[i].first; + double scale = inter_pairs[i].second; + + double fin_angle = goal.position.angle_to(start.position) + angle; + double fin_length = scale; + 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..fba23f568ff 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -4,6 +4,8 @@ #include "planning/trajectory.hpp" #include "planning/primitives/path_smoothing.hpp" +#include + namespace planning::CreatePath { /** @@ -24,4 +26,20 @@ 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); + + +const double MIN_SCALE = 0.5, MAX_SCALE = 1.5; +const double MIN_ANGLE = 20, MAX_ANGLE = 140; +const int NUM_INTERMEDIATES = 10; +const double STEP_SIZE = 0.1; + } // 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..9da93e8856f 100644 --- a/soccer/src/soccer/planning/primitives/replanner.cpp +++ b/soccer/src/soccer/planning/primitives/replanner.cpp @@ -32,9 +32,8 @@ 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); + 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 @@ -77,8 +76,8 @@ 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); + CreatePath::intermediate(params.start.linear_motion(), almost_goal, params.constraints.mot, + params.start.stamp, params.static_obstacles); } if (!path.empty()) { From 87457a9f47111e82b9379829f8ba602cba691943 Mon Sep 17 00:00:00 2001 From: rishiso Date: Sun, 1 Sep 2024 19:53:08 -0400 Subject: [PATCH 02/12] clean up and add comment --- .../src/soccer/planning/primitives/create_path.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 0535dc80fe5..4e9c2071197 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -85,7 +85,7 @@ Trajectory intermediate(const LinearMotionInstant& start, return Trajectory{{RobotInstant{Pose(start.position, 0), Twist(), start_time}}}; } - // maybe we don't need an RRT + // maybe straight line works Trajectory straight_trajectory = CreatePath::simple(start, goal, motion_constraints, start_time); @@ -97,22 +97,20 @@ Trajectory intermediate(const LinearMotionInstant& start, } + // Generate list of intermediate points std::vector intermediates = get_intermediates(start, goal); for (int i = 0; i < NUM_INTERMEDIATES; i++) { rj_geometry::Point final_inter = intermediates[i]; + // Step through the path from the robot to the final intermediate point + // and test that as an intermediate point for (double t = STEP_SIZE; t < final_inter.dist_to(start.position); t += STEP_SIZE) { - rj_geometry::Point intermediate = (final_inter - start.position).normalized(t) + start.position; - - + rj_geometry::Point intermediate = (final_inter - start.position).normalized(t) + start.position; Trajectory trajectory = CreatePath::simple(start, goal, motion_constraints, start_time, {intermediate}); - - RJ::Time first_hit_time = RJ::Time::max(), second_hit_time = RJ::Time::max();; - bool first_hits = trajectory_hits_static(trajectory, static_obstacles, start_time, &first_hit_time); - if ((!first_hits)) { + if ((!trajectory_hits_static(trajectory, static_obstacles, start_time, nullptr))) { return trajectory; } } From 5fd3f64bb964af90c64ee9b4996929444c90391e Mon Sep 17 00:00:00 2001 From: rishiso Date: Sun, 1 Sep 2024 20:54:31 -0400 Subject: [PATCH 03/12] added line test --- soccer/src/soccer/CMakeLists.txt | 1 + .../soccer/strategy/agent/position/line.cpp | 105 ++++++++++++++++++ .../soccer/strategy/agent/position/line.hpp | 26 +++++ .../agent/position/robot_factory_position.cpp | 2 + .../agent/position/robot_factory_position.hpp | 3 +- 5 files changed, 136 insertions(+), 1 deletion(-) 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 ed6a2c41e65..452c350b9e2 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/strategy/agent/position/line.cpp b/soccer/src/soccer/strategy/agent/position/line.cpp new file mode 100644 index 00000000000..249db45e200 --- /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"} { this->robot_id_ = r_id; } + +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..210e0c65f80 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/line.hpp @@ -0,0 +1,26 @@ +#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; + int robot_id_; + bool forward_ = true; + bool vertical_ = false; + bool face_target_ = false; +}; +} // namespace strategy 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..f81a3cf5f6e 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -216,6 +216,8 @@ bool RobotFactoryPosition::am_closest_kicker() { } void RobotFactoryPosition::set_default_position() { + set_current_position(); + return; // zoner defense testing // if (robot_id_ == goalie_id_) { // return; 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 a8802d828cd..ba2aab5de77 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -28,6 +28,7 @@ #include "strategy/agent/position/smartidling.hpp" #include "strategy/agent/position/solo_offense.hpp" #include "strategy/agent/position/zoner.hpp" +#include "strategy/agent/position/line.hpp" namespace strategy { @@ -146,9 +147,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&) + SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name()); current_position_->die(); current_position_ = std::make_unique(*current_position_); } From 0a6d243baa584448308d8f38510df3078996e816 Mon Sep 17 00:00:00 2001 From: rishiso Date: Sun, 1 Sep 2024 21:05:18 -0400 Subject: [PATCH 04/12] fix line issue --- soccer/src/soccer/strategy/agent/position/line.cpp | 2 +- soccer/src/soccer/strategy/agent/position/line.hpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/line.cpp b/soccer/src/soccer/strategy/agent/position/line.cpp index 249db45e200..daf99ef1112 100644 --- a/soccer/src/soccer/strategy/agent/position/line.cpp +++ b/soccer/src/soccer/strategy/agent/position/line.cpp @@ -4,7 +4,7 @@ namespace strategy { Line::Line(const Position& other) : Position{other} { position_name_ = "Line"; } -Line::Line(int r_id) : Position{r_id, "Line"} { this->robot_id_ = r_id; } +Line::Line(int r_id) : Position{r_id, "Line"} {} std::optional Line::derived_get_task(RobotIntent intent) { if (check_is_done()) { diff --git a/soccer/src/soccer/strategy/agent/position/line.hpp b/soccer/src/soccer/strategy/agent/position/line.hpp index 210e0c65f80..a47b31701a6 100644 --- a/soccer/src/soccer/strategy/agent/position/line.hpp +++ b/soccer/src/soccer/strategy/agent/position/line.hpp @@ -18,7 +18,6 @@ class Line : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - int robot_id_; bool forward_ = true; bool vertical_ = false; bool face_target_ = false; From 0796cbb13ca8e2b00fda4b8f8bf897f6ce9108f2 Mon Sep 17 00:00:00 2001 From: rishiso Date: Tue, 3 Sep 2024 19:28:17 -0400 Subject: [PATCH 05/12] made comments and removed from settle --- .../planning/planner/settle_path_planner.cpp | 8 ++++---- .../soccer/planning/primitives/create_path.cpp | 16 +++++++++++++++- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index dde32594b56..dd4f36da16c 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, + Trajectory path = CreatePath::intermediate(start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot, start_instant.stamp, - static_obstacles, dynamic_obstacles); + static_obstacles); // Calculate the RJ::Seconds buffer_duration = ball_time - path.duration(); @@ -333,8 +333,8 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn 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); + 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/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 4e9c2071197..4137791ec2d 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -105,17 +105,19 @@ Trajectory intermediate(const LinearMotionInstant& start, rj_geometry::Point final_inter = intermediates[i]; // Step through the path from the robot to the final intermediate point - // and test that as an intermediate point + // and test each point on that path as an intermediate point for (double t = STEP_SIZE; t < final_inter.dist_to(start.position); t += 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))) { return trajectory; } } } + // If all else fails, return the straight-line trajectory return straight_trajectory; } @@ -126,8 +128,13 @@ std::vector get_intermediates( 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(MIN_SCALE, MAX_SCALE); + double angle_range = MAX_ANGLE - 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; @@ -139,8 +146,13 @@ std::vector get_intermediates( angle = degrees_to_radians(angle); double scale = scale_dist(gen); + // Generate random pairs of distances and angles inter_pairs.emplace_back(angle, scale); } + + // Sort the list of pairs by the angle + // This ensures that we take paths with + // smaller offsets from the simple path sort(inter_pairs.begin(), inter_pairs.end()); for (int i = 0; i < NUM_INTERMEDIATES; i++) { @@ -149,6 +161,8 @@ std::vector get_intermediates( 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)}); } From 387b992688c121eb0334fb5a6b48da74337da1e0 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 3 Sep 2024 19:42:52 -0400 Subject: [PATCH 06/12] Fix Code Style On intermediate-pathgen (#2267) automated style fixes Co-authored-by: sanatd33 --- .../planning/planner/settle_path_planner.cpp | 12 +++---- .../planning/primitives/create_path.cpp | 34 ++++++++----------- .../planning/primitives/create_path.hpp | 20 +++++------ .../soccer/planning/primitives/replanner.cpp | 14 ++++---- .../agent/position/robot_factory_position.hpp | 2 +- 5 files changed, 37 insertions(+), 45 deletions(-) diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index dd4f36da16c..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::intermediate(start_instant.linear_motion(), target_robot_intersection, - plan_request.constraints.mot, start_instant.stamp, - static_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::intermediate(start_instant.linear_motion(), target, plan_request.constraints.mot, - start_instant.stamp, static_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/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 4137791ec2d..5da7cb09d9b 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -75,11 +75,9 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal return path; } -Trajectory intermediate(const LinearMotionInstant& start, - const LinearMotionInstant& goal, - const MotionConstraints& motion_constraints, RJ::Time start_time, - const rj_geometry::ShapeSet& static_obstacles) { - +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}}}; @@ -96,20 +94,20 @@ Trajectory intermediate(const LinearMotionInstant& start, return straight_trajectory; } - // Generate list of intermediate points std::vector intermediates = get_intermediates(start, goal); - for (int i = 0; i < 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 = STEP_SIZE; t < final_inter.dist_to(start.position); t += 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}); - + 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))) { return trajectory; @@ -121,11 +119,8 @@ Trajectory intermediate(const LinearMotionInstant& start, return straight_trajectory; } - -std::vector get_intermediates( - const LinearMotionInstant& start, - const LinearMotionInstant& goal) { - +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 @@ -147,14 +142,14 @@ std::vector get_intermediates( double scale = scale_dist(gen); // Generate random pairs of distances and angles - inter_pairs.emplace_back(angle, scale); + inter_pairs.emplace_back(angle, scale); } // Sort the list of pairs by the angle - // This ensures that we take paths with + // This ensures that we take paths with // smaller offsets from the simple path sort(inter_pairs.begin(), inter_pairs.end()); - + for (int i = 0; i < NUM_INTERMEDIATES; i++) { double angle = inter_pairs[i].first; double scale = inter_pairs[i].second; @@ -163,7 +158,8 @@ std::vector get_intermediates( 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)}); + intermediates.push_back(start.position + rj_geometry::Point{fin_length * cos(fin_angle), + fin_length * sin(fin_angle)}); } return intermediates; diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index fba23f568ff..9e67eae1e1c 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -1,10 +1,10 @@ #pragma once +#include + #include "planning/motion_constraints.hpp" -#include "planning/trajectory.hpp" #include "planning/primitives/path_smoothing.hpp" - -#include +#include "planning/trajectory.hpp" namespace planning::CreatePath { @@ -26,16 +26,12 @@ 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); +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); const double MIN_SCALE = 0.5, MAX_SCALE = 1.5; const double MIN_ANGLE = 20, MAX_ANGLE = 140; diff --git a/soccer/src/soccer/planning/primitives/replanner.cpp b/soccer/src/soccer/planning/primitives/replanner.cpp index 9da93e8856f..3c5725f5047 100644 --- a/soccer/src/soccer/planning/primitives/replanner.cpp +++ b/soccer/src/soccer/planning/primitives/replanner.cpp @@ -31,9 +31,9 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& } Trajectory pre_trajectory = partial_path(previous, params.start.stamp); - Trajectory post_trajectory = - CreatePath::intermediate(pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot, - pre_trajectory.end_time(), params.static_obstacles); + 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,7 +57,7 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory& Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { Trajectory path = CreatePath::intermediate(params.start.linear_motion(), params.goal, params.constraints.mot, - params.start.stamp, params.static_obstacles); + 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 @@ -75,9 +75,9 @@ Trajectory Replanner::full_replan(const Replanner::PlanParams& params) { almost_goal.position += shift_dir * shift_size; - path = - CreatePath::intermediate(params.start.linear_motion(), almost_goal, params.constraints.mot, - params.start.stamp, params.static_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/robot_factory_position.hpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp index ba2aab5de77..aae7765c166 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" @@ -28,7 +29,6 @@ #include "strategy/agent/position/smartidling.hpp" #include "strategy/agent/position/solo_offense.hpp" #include "strategy/agent/position/zoner.hpp" -#include "strategy/agent/position/line.hpp" namespace strategy { From 5fce23705caa6519631b7162d25fc2ece19f2ebf Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju Date: Sat, 7 Sep 2024 19:07:43 -0400 Subject: [PATCH 07/12] make pr changes --- soccer/src/soccer/planning/primitives/create_path.hpp | 8 ++++---- .../strategy/agent/position/robot_factory_position.cpp | 2 -- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index 9e67eae1e1c..20e2ce76511 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -33,9 +33,9 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst std::vector get_intermediates(const LinearMotionInstant& start, const LinearMotionInstant& goal); -const double MIN_SCALE = 0.5, MAX_SCALE = 1.5; -const double MIN_ANGLE = 20, MAX_ANGLE = 140; -const int NUM_INTERMEDIATES = 10; -const double STEP_SIZE = 0.1; +constexpr double MIN_SCALE = 0.5, MAX_SCALE = 1.5; +constexpr double MIN_ANGLE = 20, MAX_ANGLE = 140; +constexpr int NUM_INTERMEDIATES = 10; +constexpr double STEP_SIZE = 0.1; } // namespace planning::CreatePath \ No newline at end of file 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 f81a3cf5f6e..7634ff96220 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -216,8 +216,6 @@ bool RobotFactoryPosition::am_closest_kicker() { } void RobotFactoryPosition::set_default_position() { - set_current_position(); - return; // zoner defense testing // if (robot_id_ == goalie_id_) { // return; From 132f17a9cb150a15a42ceacdb15ba2fe36f8d06f Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju Date: Tue, 10 Sep 2024 18:26:11 -0400 Subject: [PATCH 08/12] add cache inter --- .../planning/primitives/create_path.cpp | 19 ++++++++++++------- .../planning/primitives/create_path.hpp | 9 ++++----- .../soccer/planning/primitives/replanner.hpp | 2 ++ 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 5da7cb09d9b..583abf4b00c 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -97,12 +97,12 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst // Generate list of intermediate points std::vector intermediates = get_intermediates(start, goal); - for (int i = 0; i < NUM_INTERMEDIATES; i++) { + for (int i = 0; i < kNumIntermediates; 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 = STEP_SIZE; t < final_inter.dist_to(start.position); t += STEP_SIZE) { + for (double t = kStepSize; t < final_inter.dist_to(start.position); t += kStepSize) { rj_geometry::Point intermediate = (final_inter - start.position).normalized(t) + start.position; Trajectory trajectory = @@ -110,6 +110,7 @@ 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))) { + cached_intermediate_pair_ = {(final_inter - start.position).angle(), (final_inter - start.position).mag()}; return trajectory; } } @@ -125,9 +126,9 @@ std::vector get_intermediates(const LinearMotionInstant& sta std::mt19937 gen(rd()); // Create a random distribution for the distance between the start // and the intermediate points - std::uniform_real_distribution<> scale_dist(MIN_SCALE, MAX_SCALE); + std::uniform_real_distribution<> scale_dist(kMinScale, kMaxScale); - double angle_range = MAX_ANGLE - MIN_ANGLE; + double angle_range = kMaxAngle - kMinAngle; // Create a random distribution for the angle between the start // and the intermediate points std::uniform_real_distribution<> angle_dist(-angle_range, angle_range); @@ -135,9 +136,9 @@ std::vector get_intermediates(const LinearMotionInstant& sta std::vector intermediates; std::vector> inter_pairs; - for (int i = 0; i < NUM_INTERMEDIATES; i++) { + for (int i = 0; i < kNumIntermediates; i++) { double angle = angle_dist(gen); - angle += std::copysign(MIN_ANGLE, angle); + angle += std::copysign(kMinAngle, angle); angle = degrees_to_radians(angle); double scale = scale_dist(gen); @@ -145,12 +146,16 @@ std::vector get_intermediates(const LinearMotionInstant& sta inter_pairs.emplace_back(angle, scale); } + if (planning::Replanner::cached_intermediate_pair_) { + inter_pairs.push_back(*cached_intermediate_pair_); + } + // Sort the list of pairs by the angle // This ensures that we take paths with // smaller offsets from the simple path sort(inter_pairs.begin(), inter_pairs.end()); - for (int i = 0; i < NUM_INTERMEDIATES; i++) { + for (int i = 0; i < kNumIntermediates; i++) { double angle = inter_pairs[i].first; double scale = inter_pairs[i].second; diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index 20e2ce76511..d90be298cbb 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -33,9 +33,8 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst std::vector get_intermediates(const LinearMotionInstant& start, const LinearMotionInstant& goal); -constexpr double MIN_SCALE = 0.5, MAX_SCALE = 1.5; -constexpr double MIN_ANGLE = 20, MAX_ANGLE = 140; -constexpr int NUM_INTERMEDIATES = 10; -constexpr double STEP_SIZE = 0.1; - +constexpr double kMinScale = 0.5, kMaxScale = 1.5; +constexpr double kMinAngle = 20, kMaxAngle = 140; +constexpr int kNumIntermediates = 10; +constexpr double kStepSize = 0.1; } // namespace planning::CreatePath \ No newline at end of file diff --git a/soccer/src/soccer/planning/primitives/replanner.hpp b/soccer/src/soccer/planning/primitives/replanner.hpp index 89d07984545..7ccfde0e022 100644 --- a/soccer/src/soccer/planning/primitives/replanner.hpp +++ b/soccer/src/soccer/planning/primitives/replanner.hpp @@ -65,6 +65,8 @@ class Replanner { return RJ::Seconds(replanner::PARAM_partial_replan_lead_time); } + std::optional> cached_intermediate_pair_; + private: // Attempt a partial replan, and use it only if it is faster. static Trajectory check_better(const PlanParams& params, From e5131ef3a32c7994a09be6ed524f4e06f6937a90 Mon Sep 17 00:00:00 2001 From: rishiso Date: Tue, 10 Sep 2024 19:47:58 -0400 Subject: [PATCH 09/12] start cacching intermediates --- soccer/src/soccer/planning/primitives/create_path.cpp | 5 ++++- soccer/src/soccer/planning/primitives/create_path.hpp | 2 +- soccer/src/soccer/planning/primitives/replanner.hpp | 2 -- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 583abf4b00c..d688796491c 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -75,6 +75,9 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal return path; } +static std::optional> cached_intermediate_pair_{}; + + Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, const MotionConstraints& motion_constraints, RJ::Time start_time, const rj_geometry::ShapeSet& static_obstacles) { @@ -146,7 +149,7 @@ std::vector get_intermediates(const LinearMotionInstant& sta inter_pairs.emplace_back(angle, scale); } - if (planning::Replanner::cached_intermediate_pair_) { + if (cached_intermediate_pair_) { inter_pairs.push_back(*cached_intermediate_pair_); } diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index d90be298cbb..14700bf1d1a 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -1,7 +1,7 @@ #pragma once #include - +#include #include "planning/motion_constraints.hpp" #include "planning/primitives/path_smoothing.hpp" #include "planning/trajectory.hpp" diff --git a/soccer/src/soccer/planning/primitives/replanner.hpp b/soccer/src/soccer/planning/primitives/replanner.hpp index 7ccfde0e022..89d07984545 100644 --- a/soccer/src/soccer/planning/primitives/replanner.hpp +++ b/soccer/src/soccer/planning/primitives/replanner.hpp @@ -65,8 +65,6 @@ class Replanner { return RJ::Seconds(replanner::PARAM_partial_replan_lead_time); } - std::optional> cached_intermediate_pair_; - private: // Attempt a partial replan, and use it only if it is faster. static Trajectory check_better(const PlanParams& params, From b053c40bc46b6443ccb93fb1b617953d19237aa8 Mon Sep 17 00:00:00 2001 From: rishiso Date: Tue, 10 Sep 2024 21:43:33 -0400 Subject: [PATCH 10/12] add params --- soccer/src/soccer/planning/planning_params.cpp | 9 +++++++++ soccer/src/soccer/planning/planning_params.hpp | 7 +++++++ .../src/soccer/planning/primitives/create_path.cpp | 14 +++++++------- .../src/soccer/planning/primitives/create_path.hpp | 5 ----- 4 files changed, 23 insertions(+), 12 deletions(-) diff --git a/soccer/src/soccer/planning/planning_params.cpp b/soccer/src/soccer/planning/planning_params.cpp index 6a6d3621fed..525c930545e 100644 --- a/soccer/src/soccer/planning/planning_params.cpp +++ b/soccer/src/soccer/planning/planning_params.cpp @@ -41,6 +41,15 @@ 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 d688796491c..666128ab4d8 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -100,12 +100,12 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst // Generate list of intermediate points std::vector intermediates = get_intermediates(start, goal); - for (int i = 0; i < kNumIntermediates; i++) { + 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 = kStepSize; t < final_inter.dist_to(start.position); t += kStepSize) { + 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 = @@ -129,9 +129,9 @@ std::vector get_intermediates(const LinearMotionInstant& sta std::mt19937 gen(rd()); // Create a random distribution for the distance between the start // and the intermediate points - std::uniform_real_distribution<> scale_dist(kMinScale, kMaxScale); + std::uniform_real_distribution<> scale_dist(intermediate::PARAM_min_scale, intermediate::PARAM_max_scale); - double angle_range = kMaxAngle - kMinAngle; + 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); @@ -139,9 +139,9 @@ std::vector get_intermediates(const LinearMotionInstant& sta std::vector intermediates; std::vector> inter_pairs; - for (int i = 0; i < kNumIntermediates; i++) { + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { double angle = angle_dist(gen); - angle += std::copysign(kMinAngle, angle); + angle += std::copysign(intermediate::PARAM_min_angle, angle); angle = degrees_to_radians(angle); double scale = scale_dist(gen); @@ -158,7 +158,7 @@ std::vector get_intermediates(const LinearMotionInstant& sta // smaller offsets from the simple path sort(inter_pairs.begin(), inter_pairs.end()); - for (int i = 0; i < kNumIntermediates; i++) { + for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { double angle = inter_pairs[i].first; double scale = inter_pairs[i].second; diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index 14700bf1d1a..400a372c649 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -32,9 +32,4 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst std::vector get_intermediates(const LinearMotionInstant& start, const LinearMotionInstant& goal); - -constexpr double kMinScale = 0.5, kMaxScale = 1.5; -constexpr double kMinAngle = 20, kMaxAngle = 140; -constexpr int kNumIntermediates = 10; -constexpr double kStepSize = 0.1; } // namespace planning::CreatePath \ No newline at end of file From e4fd024b06a00fc3ff6d6e000bb43564a86c7f37 Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 29 Sep 2024 19:26:54 -0400 Subject: [PATCH 11/12] use abs angle --- .../planning/primitives/create_path.cpp | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 666128ab4d8..02c7c810642 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -75,7 +75,7 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal return path; } -static std::optional> cached_intermediate_pair_{}; +static std::optional> cached_intermediate_tuple_{}; Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal, @@ -105,7 +105,7 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst // 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) { + 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 = @@ -113,7 +113,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))) { - cached_intermediate_pair_ = {(final_inter - start.position).angle(), (final_inter - start.position).mag()}; + auto angle = (final_inter - start.position).angle(); + cached_intermediate_tuple_ = {abs(angle), signbit(angle) ? -1 : 1, (final_inter - start.position).mag()}; return trajectory; } } @@ -137,7 +138,7 @@ std::vector get_intermediates(const LinearMotionInstant& sta std::uniform_real_distribution<> angle_dist(-angle_range, angle_range); std::vector intermediates; - std::vector> inter_pairs; + std::vector> inter_tuples; for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { double angle = angle_dist(gen); @@ -145,22 +146,22 @@ std::vector get_intermediates(const LinearMotionInstant& sta angle = degrees_to_radians(angle); double scale = scale_dist(gen); - // Generate random pairs of distances and angles - inter_pairs.emplace_back(angle, scale); + // Generate random tuples of distances and angles + inter_tuples.emplace_back(abs(angle), signbit(angle) ? -1 : 1, scale); } - if (cached_intermediate_pair_) { - inter_pairs.push_back(*cached_intermediate_pair_); + if (cached_intermediate_tuple_) { + inter_tuples.push_back(*cached_intermediate_tuple_); } - // Sort the list of pairs by the angle + // 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_pairs.begin(), inter_pairs.end()); + sort(inter_tuples.begin(), inter_tuples.end()); for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) { - double angle = inter_pairs[i].first; - double scale = inter_pairs[i].second; + 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; From 463dcce41c520de4f562da47a98a450e762a7d26 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 29 Sep 2024 19:29:32 -0400 Subject: [PATCH 12/12] Fix Code Style On intermediate-pathgen (#2269) automated style fixes Co-authored-by: sid-parikh --- .../src/soccer/planning/planning_params.cpp | 20 +++++++++++-------- .../planning/primitives/create_path.cpp | 10 ++++++---- .../planning/primitives/create_path.hpp | 3 ++- 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/soccer/src/soccer/planning/planning_params.cpp b/soccer/src/soccer/planning/planning_params.cpp index 525c930545e..e94b3abba75 100644 --- a/soccer/src/soccer/planning/planning_params.cpp +++ b/soccer/src/soccer/planning/planning_params.cpp @@ -41,14 +41,18 @@ 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, 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, diff --git a/soccer/src/soccer/planning/primitives/create_path.cpp b/soccer/src/soccer/planning/primitives/create_path.cpp index 02c7c810642..d510483a0ad 100644 --- a/soccer/src/soccer/planning/primitives/create_path.cpp +++ b/soccer/src/soccer/planning/primitives/create_path.cpp @@ -77,7 +77,6 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal 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) { @@ -105,7 +104,8 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst // 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) { + 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 = @@ -114,7 +114,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_ = {abs(angle), signbit(angle) ? -1 : 1, (final_inter - start.position).mag()}; + cached_intermediate_tuple_ = {abs(angle), signbit(angle) ? -1 : 1, + (final_inter - start.position).mag()}; return trajectory; } } @@ -130,7 +131,8 @@ std::vector get_intermediates(const LinearMotionInstant& sta 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); + 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 diff --git a/soccer/src/soccer/planning/primitives/create_path.hpp b/soccer/src/soccer/planning/primitives/create_path.hpp index 400a372c649..d71cf5809f8 100644 --- a/soccer/src/soccer/planning/primitives/create_path.hpp +++ b/soccer/src/soccer/planning/primitives/create_path.hpp @@ -1,7 +1,8 @@ #pragma once -#include #include +#include + #include "planning/motion_constraints.hpp" #include "planning/primitives/path_smoothing.hpp" #include "planning/trajectory.hpp"