From f17ea19d5242cd2732416683d2d050587b67b674 Mon Sep 17 00:00:00 2001 From: Shourik Banerjee Date: Mon, 2 Sep 2024 01:17:28 +0000 Subject: [PATCH 01/18] Created a new motion planner called rotation planner. It is a work in progress. This planner should theoretically only be rotation around a point (in our case, we are developing it to rotate about itself). --- soccer/src/soccer/CMakeLists.txt | 1 + .../planner/line_pivot_path_planner.cpp | 9 +- .../planning/planner/rotate_path_planner.cpp | 98 +++++++++++++++++++ .../planning/planner/rotate_path_planner.hpp | 42 ++++++++ .../src/soccer/planning/planner_for_robot.cpp | 2 + .../strategy/agent/position/solo_offense.cpp | 8 +- 6 files changed, 154 insertions(+), 6 deletions(-) create mode 100644 soccer/src/soccer/planning/planner/rotate_path_planner.cpp create mode 100644 soccer/src/soccer/planning/planner/rotate_path_planner.hpp diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 452c350b9e2..2e776364e2a 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -40,6 +40,7 @@ set(ROBOCUP_LIB_SRC planning/planner/path_target_path_planner.cpp planning/planner/pivot_path_planner.cpp planning/planner/line_pivot_path_planner.cpp + planning/planner/rotate_path_planner.cpp planning/planner/plan_request.cpp planning/planner/settle_path_planner.cpp planning/planner/goalie_idle_path_planner.cpp diff --git a/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp b/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp index a6b7fdd0dcd..dafd7a1a536 100644 --- a/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp @@ -22,6 +22,8 @@ Trajectory LinePivotPathPlanner::plan(const PlanRequest& request) { current_state_ = next_state(request); Trajectory path; + SPDLOG_INFO("Current state is {}", current_state_); + if (current_state_ == LINE) { path = line(request); } else { @@ -51,6 +53,7 @@ LinePivotPathPlanner::State LinePivotPathPlanner::next_state(const PlanRequest& double vel = plan_request.world_state->get_robot(true, static_cast(plan_request.shell_id)) .velocity.linear() .mag(); + if (current_state_ == LINE && (target_point.dist_to(current_point) < 0.3) && (vel < 0.3) && (!plan_request.play_state.is_stop())) { return PIVOT; @@ -93,7 +96,7 @@ Trajectory LinePivotPathPlanner::pivot(const PlanRequest& request) { const MotionCommand& command = request.motion_command; - double radius = pivot::PARAM_radius_multiplier * command.pivot_radius; + double radius = 0.1; // pivot::PARAM_radius_multiplier * command.pivot_radius; auto pivot_point = command.pivot_point; auto pivot_target = command.target.position; @@ -102,8 +105,8 @@ Trajectory LinePivotPathPlanner::pivot(const PlanRequest& request) { // max_speed = max_radians * radius MotionConstraints new_constraints = request.constraints.mot; - new_constraints.max_speed = - std::min(new_constraints.max_speed, rotation_constraints.max_speed * radius) * .5; + new_constraints.max_speed = + std::min(new_constraints.max_speed, rotation_constraints.max_speed * radius) * .5; double start_angle = pivot_point.angle_to( request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position()); diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp new file mode 100644 index 00000000000..2fd5d81813e --- /dev/null +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -0,0 +1,98 @@ +#include "rotate_path_planner.hpp" + +#include +#include + +#include +#include +#include + +#include "planning/instant.hpp" +#include "planning/planning_params.hpp" +#include "planning/primitives/angle_planning.hpp" +#include "planning/primitives/path_smoothing.hpp" +#include "planning/primitives/trapezoidal_motion.hpp" +#include "planning/primitives/velocity_profiling.hpp" +#include "planning/trajectory.hpp" + +namespace planning { +using namespace rj_geometry; + +Trajectory RotatePathPlanner::plan(const PlanRequest& request) { + return pivot(request); // type is Trajectory +} + +bool RotatePathPlanner::is_done() const { + if (!cached_angle_change_.has_value()) { + return false; + } + return abs(cached_angle_change_.value()) < degrees_to_radians(static_cast(kIsDoneAngleChangeThresh)); +} + +Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { + const RobotInstant& start_instant = request.start; + const auto& linear_constraints = request.constraints.mot; + const auto& rotation_constraints = request.constraints.rot; + + rj_geometry::ShapeSet static_obstacles; + std::vector dynamic_obstacles; + fill_obstacles(request, &static_obstacles, &dynamic_obstacles, false); + + const MotionCommand& command = request.motion_command; + + auto pivot_point = command.pivot_point; + auto pivot_target = command.target.position; + + linear_constraints.max_speed = 0; + linear_constraints.max_acceleration = 0; + + SPDLOG_INFO("Pivot point is {}", pivot_point.y()); + + auto final_position = pivot_point; // pivot_point + (pivot_point - pivot_target).normalized(); + + SPDLOG_INFO("Final position is {}", final_position.y()); + + std::vector points; + + MotionConstraints new_constraints = request.constraints.mot; + new_constraints.max_speed = std::min(new_constraints.max_speed, rotation_constraints.max_speed) * .5; + + double start_angle = pivot_point.angle_to( + request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position() + ); + double target_angle = pivot_point.angle_to(final_position); + double angle_change = fix_angle_radians(target_angle - start_angle); + + cached_angle_change_ = angle_change; + + constexpr double kMaxInterpolationRadius = 10 * M_PI / 100; + const int interpolations = std::ceil(std::abs(angle_change) / kMaxInterpolationRadius); + + points.push_back(start_instant.position()); + for (int i = 1; i <= interpolations; i++) { + double percent = (double) i / interpolations; + double angle = start_angle + angle_change * percent; + Point point = Point::direction(angle).normalized() + pivot_point; + points.push_back(point); + } + + BezierPath path_bezier(points, Point(0, 0), Point(0, 0), linear_constraints); + + Trajectory path = profile_velocity(path_bezier, start_instant.linear_velocity().mag(), 0, linear_constraints, start_instant.stamp); + + if (!Twist::nearly_equals(path.last().velocity, Twist::zero())) { + RobotInstant last; + last.position() = final_position; + last.velocity = Twist::zero(); + last.stamp = path.end_time() + 100ms; + path.append_instant(last); + } + path.hold_for(RJ::Seconds(3.0)); + + plan_angles(&path, start_instant, AngleFns::face_point(pivot_point), request.constraints.rot); + path.stamp(RJ::now()); + + return path; +} + +} \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp new file mode 100644 index 00000000000..915d7afe4ff --- /dev/null +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "path_planner.hpp" +#include "path_target_path_planner.hpp" + +namespace planning { + /** + * Path planner that only rotates the robot about a point given. + * + * Params taken from MotionCommand: + * target.position - robot will face this point when done + * target.pivot_point - robot will pivot around this point + */ + class RotatePathPlanner : public PathPlanner { + public: + RotatePathPlanner() : PathPlanner("rotate") {} + ~RotatePathPlanner() override = default; + + RotatePathPlanner(RotatePathPlanner&&) noexcept = default; + RotatePathPlanner& operator=(RotatePathPlanner&&) noexcept = default; + RotatePathPlanner(const RotatePathPlanner&) = default; + RotatePathPlanner& operator=(const RotatePathPlanner&) = default; + + Trajectory plan(const PlanRequest& request) override; + + void reset() override { + cached_angle_change_ = std::nullopt; + } + [[nodiscard]] bool is_done() const override; + + private: + Trajectory previous_; + + std::optional cached_angle_change_; // equivalent to previously recorded accorded + + PathTargetPathPlanner path_target_{}; + + Trajectory pivot(const PlanRequest& request); + + static constexpr double kIsDoneAngleChangeThresh{1.0}; + }; +} \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner_for_robot.cpp b/soccer/src/soccer/planning/planner_for_robot.cpp index 6a6cfc22042..bffcaa7255d 100644 --- a/soccer/src/soccer/planning/planner_for_robot.cpp +++ b/soccer/src/soccer/planning/planner_for_robot.cpp @@ -7,6 +7,7 @@ #include "planning/planner/intercept_path_planner.hpp" #include "planning/planner/line_kick_path_planner.hpp" #include "planning/planner/line_pivot_path_planner.hpp" +#include "planning/planner/rotate_path_planner.hpp" #include "planning/planner/path_target_path_planner.hpp" #include "planning/planner/pivot_path_planner.hpp" #include "planning/planner/settle_path_planner.hpp" @@ -32,6 +33,7 @@ PlannerForRobot::PlannerForRobot(int robot_id, rclcpp::Node* node, path_planners_[LineKickPathPlanner().name()] = std::make_unique(); path_planners_[PivotPathPlanner().name()] = std::make_unique(); path_planners_[LinePivotPathPlanner().name()] = std::make_unique(); + path_planners_[RotatePathPlanner().name()] = std::make_unique(); path_planners_[EscapeObstaclesPathPlanner().name()] = std::make_unique(); diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 3d288c8228b..7cb9fe90deb 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -53,7 +53,7 @@ SoloOffense::State SoloOffense::next_state() { case TO_BALL: { if (check_is_done()) { target_ = calculate_best_shot(); - return KICK; + return TO_BALL; } } case KICK: { @@ -81,8 +81,10 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { } case TO_BALL: { planning::LinearMotionInstant target{field_dimensions_.their_goal_loc()}; - auto pivot_cmd = planning::MotionCommand{"line_pivot", target, planning::FaceTarget{}, - false, last_world_state_->ball.position}; + auto pivot_cmd = planning::MotionCommand{"rotate", target, planning::FaceTarget{}, false, + last_world_state_->get_robot(true, robot_id_).pose.position()}; + //planning::MotionCommand{"line_pivot", target, planning::FaceTarget{}, + // false, last_world_state_->ball.position}; // get_robot(true, robot_id_).pose.position() pivot_cmd.pivot_radius = kRobotRadius * 2.5; intent.motion_command = pivot_cmd; return intent; From 63d243165acac7ce467a9285af016749290ad416 Mon Sep 17 00:00:00 2001 From: Shourik Banerjee Date: Mon, 2 Sep 2024 01:46:45 +0000 Subject: [PATCH 02/18] tested some theory about the problem. With changing the values in points, we have pinpointed the likely problem. If the two rotation points are the current pivot point and the point (0, 0), it will rotate but also translate. If the only points in the rotation are the pivot point, it will try to plan but get stuck. This is because we try to move to the same point that we are already at, so we think it is done because planners check they are done by translation. However, we need to make this one check it is done by its orientation. --- .../planning/planner/rotate_path_planner.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 2fd5d81813e..50c79736caa 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -43,14 +43,13 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { auto pivot_point = command.pivot_point; auto pivot_target = command.target.position; - linear_constraints.max_speed = 0; - linear_constraints.max_acceleration = 0; + SPDLOG_INFO("Pivot point x is {}", pivot_point.x()); + SPDLOG_INFO("Pivot point y is {}", pivot_point.y()); - SPDLOG_INFO("Pivot point is {}", pivot_point.y()); + auto final_position = pivot_point + (pivot_point - pivot_target).normalized(); - auto final_position = pivot_point; // pivot_point + (pivot_point - pivot_target).normalized(); - - SPDLOG_INFO("Final position is {}", final_position.y()); + // SPDLOG_INFO("Final position x is {}", final_position.x()); + // SPDLOG_INFO("Final position y is {}", final_position.y()); std::vector points; @@ -69,12 +68,15 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { const int interpolations = std::ceil(std::abs(angle_change) / kMaxInterpolationRadius); points.push_back(start_instant.position()); - for (int i = 1; i <= interpolations; i++) { - double percent = (double) i / interpolations; - double angle = start_angle + angle_change * percent; - Point point = Point::direction(angle).normalized() + pivot_point; - points.push_back(point); - } + points.push_back(pivot_point); + // for (int i = 1; i <= interpolations; i++) { + // double percent = (double) i / interpolations; + // double angle = start_angle + angle_change * percent; + // SPDLOG_INFO("Normalized value x is {}" , Point::direction(angle).normalized().x()); + // SPDLOG_INFO("Normalized value y is {}" , Point::direction(angle).normalized().y()); + // Point point = Point::direction(angle).normalized() + pivot_point; + // points.push_back(point); + // } BezierPath path_bezier(points, Point(0, 0), Point(0, 0), linear_constraints); From cae45746c13265306ee0c80ff0b1fc1c0bd51de8 Mon Sep 17 00:00:00 2001 From: rishiso Date: Tue, 3 Sep 2024 22:03:54 -0400 Subject: [PATCH 03/18] trapezoid angle planning --- .../src/soccer/control/trapezoidal_motion.cpp | 2 +- .../src/soccer/control/trapezoidal_motion.hpp | 36 ++++++------- .../control/trapezoidal_motion_test.cpp | 12 ++--- .../planning/planner/rotate_path_planner.cpp | 50 ++++++++----------- .../planning/primitives/angle_planning.cpp | 18 ++++++- .../planning/primitives/angle_planning.hpp | 3 ++ .../strategy/agent/position/solo_offense.cpp | 3 +- 7 files changed, 67 insertions(+), 57 deletions(-) diff --git a/soccer/src/soccer/control/trapezoidal_motion.cpp b/soccer/src/soccer/control/trapezoidal_motion.cpp index a0aee1a2044..940044f7574 100644 --- a/soccer/src/soccer/control/trapezoidal_motion.cpp +++ b/soccer/src/soccer/control/trapezoidal_motion.cpp @@ -112,7 +112,7 @@ double Trapezoidal::get_time(double distance, double path_length, double max_spe } } -bool trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap, +bool Trapezoidal::trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap, double start_speed, double final_speed, double& pos_out, double& speed_out) { // begin by assuming that there's enough time to get up to full speed diff --git a/soccer/src/soccer/control/trapezoidal_motion.hpp b/soccer/src/soccer/control/trapezoidal_motion.hpp index 742583a9762..2bedf97ab51 100644 --- a/soccer/src/soccer/control/trapezoidal_motion.hpp +++ b/soccer/src/soccer/control/trapezoidal_motion.hpp @@ -1,23 +1,5 @@ #pragma once -/** - * This function evaluates a control situation using "bang-bang". The robot - * accelerates at maxAcceleration until it hits max speed, then decelerates at - * max acc to the end point. - * - * @param[in] pathLength The total distance we are trying to travel - * @param[in] timeIntoLap How long since we started moving along the trapezoid - * @param[in] finalSpeed The speed we'd like to be going at the end of the - * trapezoid - * @param[out] distOut The distance to be at at the given time - * @param[out] speedOut The speed to be at at the given time - * @return true if the trapezoid is valid at the given time, false - * otherwise - */ -bool trapezoidal_motion(double path_length, double max_speed, double max_acc, - double time_into_lap, double start_speed, double final_speed, - double& pos_out, double& speed_out); - namespace Trapezoidal { /** * Estimates how long it would take to move to a certain distance down a path @@ -37,4 +19,22 @@ namespace Trapezoidal { double get_time(double distance, double path_length, double max_speed, double max_acc, double start_speed, double final_speed); +/** + * This function evaluates a control situation using "bang-bang". The robot + * accelerates at maxAcceleration until it hits max speed, then decelerates at + * max acc to the end point. + * + * @param[in] pathLength The total distance we are trying to travel + * @param[in] timeIntoLap How long since we started moving along the trapezoid + * @param[in] finalSpeed The speed we'd like to be going at the end of the + * trapezoid + * @param[out] distOut The distance to be at at the given time + * @param[out] speedOut The speed to be at at the given time + * @return true if the trapezoid is valid at the given time, false + * otherwise + */ +bool trapezoidal_motion(double path_length, double max_speed, double max_acc, + double time_into_lap, double start_speed, double final_speed, + double& pos_out, double& speed_out); + } // namespace Trapezoidal diff --git a/soccer/src/soccer/control/trapezoidal_motion_test.cpp b/soccer/src/soccer/control/trapezoidal_motion_test.cpp index f99a0fc70c3..969eeca5658 100644 --- a/soccer/src/soccer/control/trapezoidal_motion_test.cpp +++ b/soccer/src/soccer/control/trapezoidal_motion_test.cpp @@ -16,7 +16,7 @@ bool trapezoid1(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, pos_out, speed_out); if (valid) { @@ -34,7 +34,7 @@ bool trapezoid2(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, pos_out, speed_out); if (valid) { @@ -55,7 +55,7 @@ bool triangle1(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, pos_out, speed_out); if (valid) { @@ -76,7 +76,7 @@ bool triangle2(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, pos_out, speed_out); if (valid) { @@ -97,7 +97,7 @@ bool triangle3(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 1; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, pos_out, speed_out); if (valid) { @@ -118,7 +118,7 @@ bool triangle4(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, pos_out, speed_out); if (valid) { diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 50c79736caa..96b3bdd387e 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -40,58 +40,52 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { const MotionCommand& command = request.motion_command; - auto pivot_point = command.pivot_point; + auto pivot_point = request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position(); auto pivot_target = command.target.position; SPDLOG_INFO("Pivot point x is {}", pivot_point.x()); SPDLOG_INFO("Pivot point y is {}", pivot_point.y()); - auto final_position = pivot_point + (pivot_point - pivot_target).normalized(); - + auto final_position = pivot_point; // SPDLOG_INFO("Final position x is {}", final_position.x()); // SPDLOG_INFO("Final position y is {}", final_position.y()); std::vector points; - MotionConstraints new_constraints = request.constraints.mot; - new_constraints.max_speed = std::min(new_constraints.max_speed, rotation_constraints.max_speed) * .5; + double start_angle = + request.world_state->get_robot(true, static_cast(request.shell_id)).pose.heading(); - double start_angle = pivot_point.angle_to( - request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position() - ); - double target_angle = pivot_point.angle_to(final_position); + double target_angle = pivot_point.angle_to(pivot_target); double angle_change = fix_angle_radians(target_angle - start_angle); + SPDLOG_INFO("target anlge: {}", target_angle); + SPDLOG_INFO("current angle: {}", start_angle); cached_angle_change_ = angle_change; constexpr double kMaxInterpolationRadius = 10 * M_PI / 100; const int interpolations = std::ceil(std::abs(angle_change) / kMaxInterpolationRadius); - points.push_back(start_instant.position()); - points.push_back(pivot_point); + Trajectory path{}; + // path.append_instant(start_instant); // for (int i = 1; i <= interpolations; i++) { - // double percent = (double) i / interpolations; - // double angle = start_angle + angle_change * percent; - // SPDLOG_INFO("Normalized value x is {}" , Point::direction(angle).normalized().x()); - // SPDLOG_INFO("Normalized value y is {}" , Point::direction(angle).normalized().y()); - // Point point = Point::direction(angle).normalized() + pivot_point; - // points.push_back(point); + // auto instant = RobotInstant{Pose{start_instant.position(), }}; + // path.append_instant(instant); // } + // SPDLOG_INFO("leng: {}", points.size()); - BezierPath path_bezier(points, Point(0, 0), Point(0, 0), linear_constraints); + // BezierPath path_bezier(points, Point(0, 0), Point(0, 0), linear_constraints); - Trajectory path = profile_velocity(path_bezier, start_instant.linear_velocity().mag(), 0, linear_constraints, start_instant.stamp); + // Trajectory path = profile_velocity(path_bezier, start_instant.linear_velocity().mag(), 0, linear_constraints, start_instant.stamp); - if (!Twist::nearly_equals(path.last().velocity, Twist::zero())) { - RobotInstant last; - last.position() = final_position; - last.velocity = Twist::zero(); - last.stamp = path.end_time() + 100ms; - path.append_instant(last); - } - path.hold_for(RJ::Seconds(3.0)); + // if (!Twist::nearly_equals(path.last().velocity, Twist::zero())) { + // RobotInstant last; + // last.position() = final_position; + // last.velocity = Twist::zero(); + // last.stamp = path.end_time() + 100ms; + // path.append_instant(last); + // } - plan_angles(&path, start_instant, AngleFns::face_point(pivot_point), request.constraints.rot); + plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); path.stamp(RJ::now()); return path; diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 2b38ff6d1d8..9361f4efc43 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -6,11 +6,25 @@ namespace planning { void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, const AngleFunction& angle_function, - const RotationConstraints& /* constraints */) { + const RotationConstraints& constraints) { const RJ::Time start_time = start_instant.stamp; if (trajectory->empty()) { - throw std::invalid_argument("Cannot profile angles for empty trajectory."); + trajectory->append_instant(start_instant); + double delta_angle = angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr); + double time = Trapezoidal::get_time(delta_angle, delta_angle, constraints.max_speed, constraints.max_accel, 0, 0); + + SPDLOG_INFO("Delta Algne: {}", delta_angle); + SPDLOG_INFO("Time: {}", time); + for (int i = 1; i < time / TIME_STEP; i++) { + double pos_out = 0; + double speed_out = 0; + Trapezoidal::trapezoidal_motion(delta_angle, constraints.max_speed, constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, speed_out); + trajectory->append_instant(RobotInstant{rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading}, rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); + SPDLOG_INFO("speed_out: {}", speed_out); + } + trajectory->mark_angles_valid(); + return; } if (!trajectory->check_time(start_time)) { diff --git a/soccer/src/soccer/planning/primitives/angle_planning.hpp b/soccer/src/soccer/planning/primitives/angle_planning.hpp index 0a1dd0c40f2..dcb471d2480 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.hpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.hpp @@ -8,6 +8,7 @@ #include "planning/instant.hpp" #include "planning/robot_constraints.hpp" #include "planning/trajectory.hpp" +#include "control/trapezoidal_motion.hpp" namespace planning { @@ -114,4 +115,6 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, const AngleFunction& angle, const RotationConstraints& constraints); + +constexpr double TIME_STEP = 0.001; } // namespace planning diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 7cb9fe90deb..9e4e63d9283 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -81,8 +81,7 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { } case TO_BALL: { planning::LinearMotionInstant target{field_dimensions_.their_goal_loc()}; - auto pivot_cmd = planning::MotionCommand{"rotate", target, planning::FaceTarget{}, false, - last_world_state_->get_robot(true, robot_id_).pose.position()}; + auto pivot_cmd = planning::MotionCommand{"rotate", target, planning::FaceTarget{}, false}; //planning::MotionCommand{"line_pivot", target, planning::FaceTarget{}, // false, last_world_state_->ball.position}; // get_robot(true, robot_id_).pose.position() pivot_cmd.pivot_radius = kRobotRadius * 2.5; From 529176580a6844537089922d176f4a93e64dc27f Mon Sep 17 00:00:00 2001 From: rishiso Date: Sun, 8 Sep 2024 21:48:55 -0400 Subject: [PATCH 04/18] end 09/08 --- .../planner/path_target_path_planner.cpp | 4 ++ .../planning/planner/rotate_path_planner.cpp | 55 ++++++++----------- .../planning/planner/rotate_path_planner.hpp | 6 +- .../planning/primitives/angle_planning.cpp | 9 ++- soccer/src/soccer/radio/sim_radio.cpp | 5 ++ .../strategy/agent/position/solo_offense.cpp | 26 ++++++--- .../strategy/agent/position/solo_offense.hpp | 2 +- 7 files changed, 61 insertions(+), 46 deletions(-) diff --git a/soccer/src/soccer/planning/planner/path_target_path_planner.cpp b/soccer/src/soccer/planning/planner/path_target_path_planner.cpp index 995acbbfc1b..1a4e7b6e7c4 100644 --- a/soccer/src/soccer/planning/planner/path_target_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/path_target_path_planner.cpp @@ -87,6 +87,10 @@ AngleFunction PathTargetPathPlanner::get_angle_function(const PlanRequest& reque return AngleFns::face_angle(std::get(face_option).target); } + if (std::holds_alternative(face_option)) { + return AngleFns::face_point(request.motion_command.target.position); + } + // default to facing tangent to path // (rj_convert in motion_command.hpp also follows this default) return AngleFns::tangent; diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 96b3bdd387e..6aa511b452c 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -23,7 +23,7 @@ Trajectory RotatePathPlanner::plan(const PlanRequest& request) { } bool RotatePathPlanner::is_done() const { - if (!cached_angle_change_.has_value()) { + if (!cached_angle_change_) { return false; } return abs(cached_angle_change_.value()) < degrees_to_radians(static_cast(kIsDoneAngleChangeThresh)); @@ -46,47 +46,40 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { SPDLOG_INFO("Pivot point x is {}", pivot_point.x()); SPDLOG_INFO("Pivot point y is {}", pivot_point.y()); - auto final_position = pivot_point; - // SPDLOG_INFO("Final position x is {}", final_position.x()); - // SPDLOG_INFO("Final position y is {}", final_position.y()); - - std::vector points; - double start_angle = request.world_state->get_robot(true, static_cast(request.shell_id)).pose.heading(); double target_angle = pivot_point.angle_to(pivot_target); double angle_change = fix_angle_radians(target_angle - start_angle); - SPDLOG_INFO("target anlge: {}", target_angle); SPDLOG_INFO("current angle: {}", start_angle); + SPDLOG_INFO("cached_target_agne: {}", cached_target_angle_.value_or(0)); + SPDLOG_INFO("target angle: {}", target_angle); cached_angle_change_ = angle_change; + SPDLOG_INFO("cached angle change: {}", *cached_angle_change_); - constexpr double kMaxInterpolationRadius = 10 * M_PI / 100; - const int interpolations = std::ceil(std::abs(angle_change) / kMaxInterpolationRadius); Trajectory path{}; - // path.append_instant(start_instant); - // for (int i = 1; i <= interpolations; i++) { - // auto instant = RobotInstant{Pose{start_instant.position(), }}; - // path.append_instant(instant); - // } - // SPDLOG_INFO("leng: {}", points.size()); - - // BezierPath path_bezier(points, Point(0, 0), Point(0, 0), linear_constraints); - - // Trajectory path = profile_velocity(path_bezier, start_instant.linear_velocity().mag(), 0, linear_constraints, start_instant.stamp); - - // if (!Twist::nearly_equals(path.last().velocity, Twist::zero())) { - // RobotInstant last; - // last.position() = final_position; - // last.velocity = Twist::zero(); - // last.stamp = path.end_time() + 100ms; - // path.append_instant(last); - // } - - plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); - path.stamp(RJ::now()); + + if (abs(*cached_target_angle_ - target_angle) < degrees_to_radians(1)) { + if (cached_path_) { + path = cached_path_.value(); + } + else { + SPDLOG_INFO("reset"); + plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); + path.stamp(RJ::now()); + cached_path_ = path; + } + } else { + SPDLOG_INFO("reset"); + cached_path_.reset(); + plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); + path.stamp(RJ::now()); + cached_path_ = path; + } + + cached_target_angle_ = target_angle; return path; } diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp index 915d7afe4ff..726514a969c 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp @@ -24,6 +24,7 @@ namespace planning { Trajectory plan(const PlanRequest& request) override; void reset() override { + cached_target_angle_ = std::nullopt; cached_angle_change_ = std::nullopt; } [[nodiscard]] bool is_done() const override; @@ -31,7 +32,10 @@ namespace planning { private: Trajectory previous_; - std::optional cached_angle_change_; // equivalent to previously recorded accorded + std::optional cached_target_angle_; // equivalent to previously recorded accorded + std::optional cached_angle_change_; + + std::optional cached_path_; PathTargetPathPlanner path_target_{}; diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 9361f4efc43..677f305e66d 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -11,17 +11,16 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, if (trajectory->empty()) { trajectory->append_instant(start_instant); - double delta_angle = angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr); + double delta_angle = angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr) - start_instant.heading(); double time = Trapezoidal::get_time(delta_angle, delta_angle, constraints.max_speed, constraints.max_accel, 0, 0); - SPDLOG_INFO("Delta Algne: {}", delta_angle); - SPDLOG_INFO("Time: {}", time); + SPDLOG_INFO("delta angle: {}", delta_angle); for (int i = 1; i < time / TIME_STEP; i++) { double pos_out = 0; double speed_out = 0; Trapezoidal::trapezoidal_motion(delta_angle, constraints.max_speed, constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, speed_out); - trajectory->append_instant(RobotInstant{rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading}, rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); - SPDLOG_INFO("speed_out: {}", speed_out); + trajectory->append_instant(RobotInstant{rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); + // SPDLOG_INFO("speed_out: {}", speed_out); } trajectory->mark_angles_valid(); return; diff --git a/soccer/src/soccer/radio/sim_radio.cpp b/soccer/src/soccer/radio/sim_radio.cpp index 4bde12611f2..1adcd6346c2 100644 --- a/soccer/src/soccer/radio/sim_radio.cpp +++ b/soccer/src/soccer/radio/sim_radio.cpp @@ -145,6 +145,11 @@ void SimRadio::send_control_message(uint8_t robot_id, const rj_msgs::msg::Motion /* sim_robot->dribbler_speed()); */ /* } */ + + if (robot_id = 2) { + SPDLOG_INFO("manip spee: {}", manipulator.dribbler_speed); + SPDLOG_INFO("dribbler speed: {}", sim_robot->dribbler_speed()); + } socket_.send_to(buffer(out), robot_control_endpoint_); } diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 9e4e63d9283..80d9e8c457e 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -11,9 +11,9 @@ SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {} std::optional SoloOffense::derived_get_task(RobotIntent intent) { // Get next state, and if different, reset clock State new_state = next_state(); - if (new_state != current_state_) { - // SPDLOG_INFO("New State: {}", std::to_string(static_cast(new_state))); - } + // if (new_state != current_state_) { + // } + SPDLOG_INFO("New State: {}", std::to_string(static_cast(new_state))); current_state_ = new_state; // Calculate task based on state @@ -51,13 +51,19 @@ SoloOffense::State SoloOffense::next_state() { return TO_BALL; } case TO_BALL: { + SPDLOG_INFO("{}", (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag()); + if ((last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() < kRobotDiameter){ + return ROTATE; + } + } + case ROTATE: { if (check_is_done()) { target_ = calculate_best_shot(); - return TO_BALL; + return KICK; } } case KICK: { - if (check_is_done()) { + if (check_is_done() || (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() > kRobotDiameter * 3) { return TO_BALL; } } @@ -80,12 +86,16 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { return intent; } case TO_BALL: { + planning::LinearMotionInstant target{last_world_state_->ball.position}; + auto pivot_cmd = planning::MotionCommand{"path_target", target, planning::FaceTarget{}, false}; + intent.motion_command = pivot_cmd; + return intent; + } + case ROTATE: { planning::LinearMotionInstant target{field_dimensions_.their_goal_loc()}; auto pivot_cmd = planning::MotionCommand{"rotate", target, planning::FaceTarget{}, false}; - //planning::MotionCommand{"line_pivot", target, planning::FaceTarget{}, - // false, last_world_state_->ball.position}; // get_robot(true, robot_id_).pose.position() - pivot_cmd.pivot_radius = kRobotRadius * 2.5; intent.motion_command = pivot_cmd; + intent.dribbler_speed = 1; return intent; } case KICK: { diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.hpp b/soccer/src/soccer/strategy/agent/position/solo_offense.hpp index cbbd48b0e49..1f68bd8333c 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.hpp @@ -36,7 +36,7 @@ class SoloOffense : public Position { */ std::optional derived_get_task(RobotIntent intent) override; - enum State { TO_BALL, KICK, MARKER }; + enum State { TO_BALL, KICK, MARKER, ROTATE }; State current_state_ = TO_BALL; From 3a9b19f39da4d1057c7a8c8f5fde8b437df49e80 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sun, 29 Sep 2024 19:34:39 -0400 Subject: [PATCH 05/18] updated files --- install/setup.bash | 2 +- install/setup.zsh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/install/setup.bash b/install/setup.bash index b84bbe02bab..6e2b3722020 100755 --- a/install/setup.bash +++ b/install/setup.bash @@ -34,7 +34,7 @@ _pythonpath_add() { # No direct replacement for the command exists, and in Ubuntu 22.04, # this script will produce a warning about this. We may need to find a # replacement library in the future. -_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))") +_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))") _pythonpath_add "${_PYTHON_LIB_PATH}" unset _PYTHON_LIB_PATH diff --git a/install/setup.zsh b/install/setup.zsh index 4d6b7f16a21..db45a72c353 100644 --- a/install/setup.zsh +++ b/install/setup.zsh @@ -30,7 +30,7 @@ _pythonpath_add() { fi } -_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))") +_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))") _pythonpath_add "${_PYTHON_LIB_PATH}" unset _PYTHON_LIB_PATH From 7c23aae5908cfe00e37128e7a2d283489bca3742 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Tue, 1 Oct 2024 20:50:53 -0400 Subject: [PATCH 06/18] Dribble in Sim (#2287) * dribbler is stupid * use realism * add realism to simoulator cli run --------- Co-authored-by: petergarud --- launch/framework.bash | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/framework.bash b/launch/framework.bash index bf67387d40a..b5dcc9e9f77 100755 --- a/launch/framework.bash +++ b/launch/framework.bash @@ -10,7 +10,7 @@ if [ -z "$binary" ]; then fi # Run the binary in the background -"$binary" & +"$binary" -g 2020B --realism RC2021 & binary_pid=$! # Ensure that pressing Ctrl+C kills all subprocesses From 4a67e5fa7ec7d2c39a09f4b43415e298aa0e9d64 Mon Sep 17 00:00:00 2001 From: Sid Parikh Date: Sun, 6 Oct 2024 19:00:27 -0400 Subject: [PATCH 07/18] add ros env vars to source script (#2288) --- .gitignore | 1 - install/env.sh | 5 +++++ makefile | 6 +++--- source.bash | 3 +++ 4 files changed, 11 insertions(+), 4 deletions(-) create mode 100644 install/env.sh diff --git a/.gitignore b/.gitignore index 0954366ef7e..176668b4431 100644 --- a/.gitignore +++ b/.gitignore @@ -6,7 +6,6 @@ # build directories /build -/install /gmon.out *.swp /.settings diff --git a/install/env.sh b/install/env.sh new file mode 100644 index 00000000000..e9bc1810806 --- /dev/null +++ b/install/env.sh @@ -0,0 +1,5 @@ +# Here are some useful environment variables for setting up a consistent ROS environment +# some information on this can be found here: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html +export ROS_DOMAIN_ID=19 # this is a random number, it's just important to keep it consistent across shells +export ROS_LOCALHOST_ONLY=1 # for our stack specifically, this is quite helpful. + diff --git a/makefile b/makefile index 4ddb5959fb1..837c56b2949 100644 --- a/makefile +++ b/makefile @@ -72,11 +72,11 @@ run-sim: # run our stack with default flags # TODO: actually name our software stack something run-our-stack: - ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True + ros2 launch rj_robocup soccer.launch.py run_sim:=True # run sim with external referee (SSL Game Controller) run-sim-external: - ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False + ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False run-sim-ex: run-sim-external @@ -95,7 +95,7 @@ run-manual: # same as run-real, with different server port run-alt-real: - ROS_DOMAIN_ID=2 ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b + ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b # run sim2play (requires external referee) run-sim2play: diff --git a/source.bash b/source.bash index 85a5e0bedf1..d86ad19995f 100755 --- a/source.bash +++ b/source.bash @@ -9,3 +9,6 @@ if [[ $SHELL == *"zsh"* ]]; then source /opt/ros/humble/setup.zsh source install/setup.zsh fi + +source install/env.sh + From 449b1e461cc47ff827ce079b4120c6d6fc91829b Mon Sep 17 00:00:00 2001 From: Jack <65086686+jacksherling@users.noreply.github.com> Date: Sun, 6 Oct 2024 19:02:08 -0400 Subject: [PATCH 08/18] Update tutorial.rst (#2286) --- docs/source/tutorial.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/source/tutorial.rst b/docs/source/tutorial.rst index b44e331c40e..d836f27c95e 100644 --- a/docs/source/tutorial.rst +++ b/docs/source/tutorial.rst @@ -351,6 +351,7 @@ but here are some more hints. * The motion command for driving in a straight line is :cpp:`"path_target"`. * You will probably need to override some methods relating to passing, but you can leave their implementations empty. They don't need to do anything in your position, as your robot will not pass the ball * The simulator tells you the coordinates of your cursor—these are the same coordinates you can use in your motion commands. +* You will need to add the new file name you create to ``soccer/src/soccer/CMakeLists.txt``. See how this is done for other positions. Testing ~~~~~~~ @@ -668,4 +669,4 @@ Here are all the external links from this document, copied again for your easy r * `C++ Inheritance`_ -.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/ \ No newline at end of file +.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/ From cd24c47fada86fda00dd772ee7034379a1e73a0e Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sun, 27 Oct 2024 20:35:33 -0400 Subject: [PATCH 09/18] fixed rebase issues --- .../planning/planner/rotate_path_planner.cpp | 22 ++++----- soccer/src/soccer/radio/sim_radio.cpp | 9 +--- .../strategy/agent/position/solo_offense.cpp | 48 ++++++++++++++----- .../strategy/agent/position/solo_offense.hpp | 3 ++ 4 files changed, 52 insertions(+), 30 deletions(-) diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 6aa511b452c..17b2c5b82f7 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -43,20 +43,20 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { auto pivot_point = request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position(); auto pivot_target = command.target.position; - SPDLOG_INFO("Pivot point x is {}", pivot_point.x()); - SPDLOG_INFO("Pivot point y is {}", pivot_point.y()); + // SPDLOG_INFO("Pivot point x is {}", pivot_point.x()); + // SPDLOG_INFO("Pivot point y is {}", pivot_point.y()); - double start_angle = + double start_angle = request.world_state->get_robot(true, static_cast(request.shell_id)).pose.heading(); double target_angle = pivot_point.angle_to(pivot_target); double angle_change = fix_angle_radians(target_angle - start_angle); - SPDLOG_INFO("current angle: {}", start_angle); - SPDLOG_INFO("cached_target_agne: {}", cached_target_angle_.value_or(0)); - SPDLOG_INFO("target angle: {}", target_angle); + // SPDLOG_INFO("current angle: {}", start_angle); + // SPDLOG_INFO("cached_target_agne: {}", cached_target_angle_.value_or(0)); + // SPDLOG_INFO("target angle: {}", target_angle); cached_angle_change_ = angle_change; - SPDLOG_INFO("cached angle change: {}", *cached_angle_change_); + // SPDLOG_INFO("cached angle change: {}", *cached_angle_change_); Trajectory path{}; @@ -66,13 +66,13 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { path = cached_path_.value(); } else { - SPDLOG_INFO("reset"); + // SPDLOG_INFO("reset"); plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); path.stamp(RJ::now()); cached_path_ = path; - } + } } else { - SPDLOG_INFO("reset"); + // SPDLOG_INFO("reset"); cached_path_.reset(); plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); path.stamp(RJ::now()); @@ -84,4 +84,4 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { return path; } -} \ No newline at end of file +} diff --git a/soccer/src/soccer/radio/sim_radio.cpp b/soccer/src/soccer/radio/sim_radio.cpp index 1adcd6346c2..3084fcfd7c1 100644 --- a/soccer/src/soccer/radio/sim_radio.cpp +++ b/soccer/src/soccer/radio/sim_radio.cpp @@ -88,7 +88,7 @@ SimRadio::SimRadio(bool blue_team) // file probably an issue with the hacky way I got param files to dynamically load std::string localhost = "127.0.0.1"; this->get_parameter_or("interface", param_radio_interface_, localhost); - SPDLOG_INFO("SimRadio param_radio_interface_ {}", param_radio_interface_); + // SPDLOG_INFO("SimRadio param_radio_interface_ {}", param_radio_interface_); address_ = boost::asio::ip::make_address(param_radio_interface_).to_v4(); robot_control_endpoint_ = ip::udp::endpoint(address_, blue_team_ ? kSimBlueCommandPort : kSimYellowCommandPort); @@ -144,12 +144,7 @@ void SimRadio::send_control_message(uint8_t robot_id, const rj_msgs::msg::Motion /* SPDLOG_ERROR("sim_robot: {} {} {} \n", sim_robot->id(), sim_robot->kick_speed(), */ /* sim_robot->dribbler_speed()); */ /* } */ - - - if (robot_id = 2) { - SPDLOG_INFO("manip spee: {}", manipulator.dribbler_speed); - SPDLOG_INFO("dribbler speed: {}", sim_robot->dribbler_speed()); - } + socket_.send_to(buffer(out), robot_control_endpoint_); } diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 80d9e8c457e..32a31fc0e75 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -51,21 +51,31 @@ SoloOffense::State SoloOffense::next_state() { return TO_BALL; } case TO_BALL: { - SPDLOG_INFO("{}", (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag()); - if ((last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() < kRobotDiameter){ + // SPDLOG_INFO("{}", (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag()); + // if ((last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() < kRobotDiameter){ + // return ROTATE; + // } + if (check_is_done()) { return ROTATE; } + return TO_BALL; } case ROTATE: { if (check_is_done()) { - target_ = calculate_best_shot(); + counter_ = 0; + kick_ = true; return KICK; } + return ROTATE; } case KICK: { - if (check_is_done() || (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() > kRobotDiameter * 3) { + // if (check_is_done() || (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() > kRobotDiameter * 3) { + // return TO_BALL; + // } + if (!kick_ || (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() > kRobotRadius * 5) { return TO_BALL; } + return KICK; } } return current_state_; @@ -86,26 +96,40 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { return intent; } case TO_BALL: { - planning::LinearMotionInstant target{last_world_state_->ball.position}; - auto pivot_cmd = planning::MotionCommand{"path_target", target, planning::FaceTarget{}, false}; + rj_geometry::Point robotToBall = (last_world_state_->ball.position - last_world_state_->get_robot(true, robot_id_).pose.position()); + double slowDown = 1.0; + double length = robotToBall.mag() - kRobotRadius * slowDown; + robotToBall = robotToBall.normalized(length); + 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}; intent.motion_command = pivot_cmd; + intent.dribbler_speed = 255; return intent; } case ROTATE: { - planning::LinearMotionInstant target{field_dimensions_.their_goal_loc()}; + planning::LinearMotionInstant target{calculate_best_shot()}; auto pivot_cmd = planning::MotionCommand{"rotate", target, planning::FaceTarget{}, false}; intent.motion_command = pivot_cmd; - intent.dribbler_speed = 1; + intent.dribbler_speed = 255; return intent; } case KICK: { - auto line_kick_cmd = - planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; - - intent.motion_command = line_kick_cmd; + // double scaleFactor = 0.1; + // rj_geometry::Point point = (last_world_state_->ball.position - last_world_state_->get_robot(true, robot_id_).pose.position()).normalized(scaleFactor); + // point += last_world_state_->get_robot(true, robot_id_).pose.position(); + // planning::LinearMotionInstant target{point}; + // SPDLOG_INFO("KICK KICK KICK"); + 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}; + intent.motion_command = kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; intent.kick_speed = 4.0; + counter_++; + if (counter_ > 15) { + kick_ = false; + } return intent; } diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.hpp b/soccer/src/soccer/strategy/agent/position/solo_offense.hpp index 1f68bd8333c..4f4608dc9e8 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.hpp @@ -44,6 +44,9 @@ class SoloOffense : public Position { int marking_id_; + bool kick_ = false; + int counter_ = 0; + /** * @return what the state should be right now. called on each get_task tick */ From 8eb69911eb497a1e67986c6f8180297e15079916 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 27 Oct 2024 20:46:30 -0400 Subject: [PATCH 10/18] Fix Code Style On rotate-kick (#2300) automated style fixes Co-authored-by: Squid5678 --- .../src/soccer/control/trapezoidal_motion.cpp | 6 +- .../src/soccer/control/trapezoidal_motion.hpp | 5 +- .../control/trapezoidal_motion_test.cpp | 24 +++--- .../planner/line_pivot_path_planner.cpp | 6 +- .../planning/planner/rotate_path_planner.cpp | 22 ++--- .../planning/planner/rotate_path_planner.hpp | 80 +++++++++---------- .../src/soccer/planning/planner_for_robot.cpp | 2 +- .../planning/primitives/angle_planning.cpp | 19 +++-- .../planning/primitives/angle_planning.hpp | 3 +- soccer/src/soccer/radio/sim_radio.cpp | 2 +- .../strategy/agent/position/solo_offense.cpp | 34 +++++--- 11 files changed, 111 insertions(+), 92 deletions(-) diff --git a/soccer/src/soccer/control/trapezoidal_motion.cpp b/soccer/src/soccer/control/trapezoidal_motion.cpp index 940044f7574..c4aded7d6f1 100644 --- a/soccer/src/soccer/control/trapezoidal_motion.cpp +++ b/soccer/src/soccer/control/trapezoidal_motion.cpp @@ -112,9 +112,9 @@ double Trapezoidal::get_time(double distance, double path_length, double max_spe } } -bool Trapezoidal::trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap, - double start_speed, double final_speed, double& pos_out, - double& speed_out) { +bool Trapezoidal::trapezoidal_motion(double path_length, double max_speed, double max_acc, + double time_into_lap, double start_speed, double final_speed, + double& pos_out, double& speed_out) { // begin by assuming that there's enough time to get up to full speed // we do this by calculating the full ramp-up and ramp-down, then seeing // if the distance travelled is too great. If it's gone too far, this is diff --git a/soccer/src/soccer/control/trapezoidal_motion.hpp b/soccer/src/soccer/control/trapezoidal_motion.hpp index 2bedf97ab51..9ac1091bdc3 100644 --- a/soccer/src/soccer/control/trapezoidal_motion.hpp +++ b/soccer/src/soccer/control/trapezoidal_motion.hpp @@ -33,8 +33,7 @@ double get_time(double distance, double path_length, double max_speed, * @return true if the trapezoid is valid at the given time, false * otherwise */ -bool trapezoidal_motion(double path_length, double max_speed, double max_acc, - double time_into_lap, double start_speed, double final_speed, - double& pos_out, double& speed_out); +bool trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap, + double start_speed, double final_speed, double& pos_out, double& speed_out); } // namespace Trapezoidal diff --git a/soccer/src/soccer/control/trapezoidal_motion_test.cpp b/soccer/src/soccer/control/trapezoidal_motion_test.cpp index 969eeca5658..f9d10d8dc74 100644 --- a/soccer/src/soccer/control/trapezoidal_motion_test.cpp +++ b/soccer/src/soccer/control/trapezoidal_motion_test.cpp @@ -16,8 +16,8 @@ bool trapezoid1(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { auto time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -34,8 +34,8 @@ bool trapezoid2(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 0; - bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -55,8 +55,8 @@ bool triangle1(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -76,8 +76,8 @@ bool triangle2(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -97,8 +97,8 @@ bool triangle3(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 1; - bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -118,8 +118,8 @@ bool triangle4(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 0; - bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, diff --git a/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp b/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp index dafd7a1a536..b76f31fefd2 100644 --- a/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp @@ -96,7 +96,7 @@ Trajectory LinePivotPathPlanner::pivot(const PlanRequest& request) { const MotionCommand& command = request.motion_command; - double radius = 0.1; // pivot::PARAM_radius_multiplier * command.pivot_radius; + double radius = 0.1; // pivot::PARAM_radius_multiplier * command.pivot_radius; auto pivot_point = command.pivot_point; auto pivot_target = command.target.position; @@ -105,8 +105,8 @@ Trajectory LinePivotPathPlanner::pivot(const PlanRequest& request) { // max_speed = max_radians * radius MotionConstraints new_constraints = request.constraints.mot; - new_constraints.max_speed = - std::min(new_constraints.max_speed, rotation_constraints.max_speed * radius) * .5; + new_constraints.max_speed = + std::min(new_constraints.max_speed, rotation_constraints.max_speed * radius) * .5; double start_angle = pivot_point.angle_to( request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position()); diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 17b2c5b82f7..924288ea539 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -19,14 +19,15 @@ namespace planning { using namespace rj_geometry; Trajectory RotatePathPlanner::plan(const PlanRequest& request) { - return pivot(request); // type is Trajectory + return pivot(request); // type is Trajectory } bool RotatePathPlanner::is_done() const { if (!cached_angle_change_) { return false; } - return abs(cached_angle_change_.value()) < degrees_to_radians(static_cast(kIsDoneAngleChangeThresh)); + return abs(cached_angle_change_.value()) < + degrees_to_radians(static_cast(kIsDoneAngleChangeThresh)); } Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { @@ -40,7 +41,8 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { const MotionCommand& command = request.motion_command; - auto pivot_point = request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position(); + auto pivot_point = + request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position(); auto pivot_target = command.target.position; // SPDLOG_INFO("Pivot point x is {}", pivot_point.x()); @@ -58,23 +60,23 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { cached_angle_change_ = angle_change; // SPDLOG_INFO("cached angle change: {}", *cached_angle_change_); - Trajectory path{}; if (abs(*cached_target_angle_ - target_angle) < degrees_to_radians(1)) { if (cached_path_) { - path = cached_path_.value(); - } - else { + path = cached_path_.value(); + } else { // SPDLOG_INFO("reset"); - plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); + plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), + request.constraints.rot); path.stamp(RJ::now()); cached_path_ = path; } } else { // SPDLOG_INFO("reset"); cached_path_.reset(); - plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); + plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), + request.constraints.rot); path.stamp(RJ::now()); cached_path_ = path; } @@ -84,4 +86,4 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { return path; } -} +} // namespace planning diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp index 726514a969c..7b2e5c86ad5 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp @@ -4,43 +4,43 @@ #include "path_target_path_planner.hpp" namespace planning { - /** - * Path planner that only rotates the robot about a point given. - * - * Params taken from MotionCommand: - * target.position - robot will face this point when done - * target.pivot_point - robot will pivot around this point - */ - class RotatePathPlanner : public PathPlanner { - public: - RotatePathPlanner() : PathPlanner("rotate") {} - ~RotatePathPlanner() override = default; - - RotatePathPlanner(RotatePathPlanner&&) noexcept = default; - RotatePathPlanner& operator=(RotatePathPlanner&&) noexcept = default; - RotatePathPlanner(const RotatePathPlanner&) = default; - RotatePathPlanner& operator=(const RotatePathPlanner&) = default; - - Trajectory plan(const PlanRequest& request) override; - - void reset() override { - cached_target_angle_ = std::nullopt; - cached_angle_change_ = std::nullopt; - } - [[nodiscard]] bool is_done() const override; - - private: - Trajectory previous_; - - std::optional cached_target_angle_; // equivalent to previously recorded accorded - std::optional cached_angle_change_; - - std::optional cached_path_; - - PathTargetPathPlanner path_target_{}; - - Trajectory pivot(const PlanRequest& request); - - static constexpr double kIsDoneAngleChangeThresh{1.0}; - }; -} \ No newline at end of file +/** + * Path planner that only rotates the robot about a point given. + * + * Params taken from MotionCommand: + * target.position - robot will face this point when done + * target.pivot_point - robot will pivot around this point + */ +class RotatePathPlanner : public PathPlanner { +public: + RotatePathPlanner() : PathPlanner("rotate") {} + ~RotatePathPlanner() override = default; + + RotatePathPlanner(RotatePathPlanner&&) noexcept = default; + RotatePathPlanner& operator=(RotatePathPlanner&&) noexcept = default; + RotatePathPlanner(const RotatePathPlanner&) = default; + RotatePathPlanner& operator=(const RotatePathPlanner&) = default; + + Trajectory plan(const PlanRequest& request) override; + + void reset() override { + cached_target_angle_ = std::nullopt; + cached_angle_change_ = std::nullopt; + } + [[nodiscard]] bool is_done() const override; + +private: + Trajectory previous_; + + std::optional cached_target_angle_; // equivalent to previously recorded accorded + std::optional cached_angle_change_; + + std::optional cached_path_; + + PathTargetPathPlanner path_target_{}; + + Trajectory pivot(const PlanRequest& request); + + static constexpr double kIsDoneAngleChangeThresh{1.0}; +}; +} // namespace planning \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner_for_robot.cpp b/soccer/src/soccer/planning/planner_for_robot.cpp index bffcaa7255d..7a302773c2f 100644 --- a/soccer/src/soccer/planning/planner_for_robot.cpp +++ b/soccer/src/soccer/planning/planner_for_robot.cpp @@ -7,9 +7,9 @@ #include "planning/planner/intercept_path_planner.hpp" #include "planning/planner/line_kick_path_planner.hpp" #include "planning/planner/line_pivot_path_planner.hpp" -#include "planning/planner/rotate_path_planner.hpp" #include "planning/planner/path_target_path_planner.hpp" #include "planning/planner/pivot_path_planner.hpp" +#include "planning/planner/rotate_path_planner.hpp" #include "planning/planner/settle_path_planner.hpp" namespace planning { diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 677f305e66d..991072338c6 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -5,21 +5,28 @@ namespace planning { void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, - const AngleFunction& angle_function, - const RotationConstraints& constraints) { + const AngleFunction& angle_function, const RotationConstraints& constraints) { const RJ::Time start_time = start_instant.stamp; if (trajectory->empty()) { trajectory->append_instant(start_instant); - double delta_angle = angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr) - start_instant.heading(); - double time = Trapezoidal::get_time(delta_angle, delta_angle, constraints.max_speed, constraints.max_accel, 0, 0); + double delta_angle = + angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr) - + start_instant.heading(); + double time = Trapezoidal::get_time(delta_angle, delta_angle, constraints.max_speed, + constraints.max_accel, 0, 0); SPDLOG_INFO("delta angle: {}", delta_angle); for (int i = 1; i < time / TIME_STEP; i++) { double pos_out = 0; double speed_out = 0; - Trapezoidal::trapezoidal_motion(delta_angle, constraints.max_speed, constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, speed_out); - trajectory->append_instant(RobotInstant{rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); + Trapezoidal::trapezoidal_motion(delta_angle, constraints.max_speed, + constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, + speed_out); + trajectory->append_instant(RobotInstant{ + rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, + rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, + start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); // SPDLOG_INFO("speed_out: {}", speed_out); } trajectory->mark_angles_valid(); diff --git a/soccer/src/soccer/planning/primitives/angle_planning.hpp b/soccer/src/soccer/planning/primitives/angle_planning.hpp index dcb471d2480..3a39d004f57 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.hpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.hpp @@ -5,10 +5,10 @@ #include #include +#include "control/trapezoidal_motion.hpp" #include "planning/instant.hpp" #include "planning/robot_constraints.hpp" #include "planning/trajectory.hpp" -#include "control/trapezoidal_motion.hpp" namespace planning { @@ -115,6 +115,5 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, const AngleFunction& angle, const RotationConstraints& constraints); - constexpr double TIME_STEP = 0.001; } // namespace planning diff --git a/soccer/src/soccer/radio/sim_radio.cpp b/soccer/src/soccer/radio/sim_radio.cpp index 3084fcfd7c1..3375a947ab0 100644 --- a/soccer/src/soccer/radio/sim_radio.cpp +++ b/soccer/src/soccer/radio/sim_radio.cpp @@ -144,7 +144,7 @@ void SimRadio::send_control_message(uint8_t robot_id, const rj_msgs::msg::Motion /* SPDLOG_ERROR("sim_robot: {} {} {} \n", sim_robot->id(), sim_robot->kick_speed(), */ /* sim_robot->dribbler_speed()); */ /* } */ - + socket_.send_to(buffer(out), robot_control_endpoint_); } diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 32a31fc0e75..7901189795e 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -51,8 +51,9 @@ SoloOffense::State SoloOffense::next_state() { return TO_BALL; } case TO_BALL: { - // SPDLOG_INFO("{}", (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag()); - // if ((last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() < kRobotDiameter){ + // SPDLOG_INFO("{}", (last_world_state_->get_robot(true, robot_id_).pose.position() - + // current_point).mag()); if ((last_world_state_->get_robot(true, + // robot_id_).pose.position() - current_point).mag() < kRobotDiameter){ // return ROTATE; // } if (check_is_done()) { @@ -69,10 +70,13 @@ SoloOffense::State SoloOffense::next_state() { return ROTATE; } case KICK: { - // if (check_is_done() || (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() > kRobotDiameter * 3) { + // if (check_is_done() || (last_world_state_->get_robot(true, robot_id_).pose.position() + // - current_point).mag() > kRobotDiameter * 3) { // return TO_BALL; // } - if (!kick_ || (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point).mag() > kRobotRadius * 5) { + if (!kick_ || + (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point) + .mag() > kRobotRadius * 5) { return TO_BALL; } return KICK; @@ -96,32 +100,40 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { return intent; } case TO_BALL: { - rj_geometry::Point robotToBall = (last_world_state_->ball.position - last_world_state_->get_robot(true, robot_id_).pose.position()); + rj_geometry::Point robotToBall = + (last_world_state_->ball.position - + last_world_state_->get_robot(true, robot_id_).pose.position()); double slowDown = 1.0; double length = robotToBall.mag() - kRobotRadius * slowDown; robotToBall = robotToBall.normalized(length); - 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::LinearMotionInstant target{ + last_world_state_->get_robot(true, robot_id_).pose.position() + robotToBall}; + auto pivot_cmd = + planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true}; intent.motion_command = pivot_cmd; intent.dribbler_speed = 255; return intent; } case ROTATE: { planning::LinearMotionInstant target{calculate_best_shot()}; - auto pivot_cmd = planning::MotionCommand{"rotate", target, planning::FaceTarget{}, false}; + auto pivot_cmd = + planning::MotionCommand{"rotate", target, planning::FaceTarget{}, false}; intent.motion_command = pivot_cmd; intent.dribbler_speed = 255; return intent; } case KICK: { // double scaleFactor = 0.1; - // rj_geometry::Point point = (last_world_state_->ball.position - last_world_state_->get_robot(true, robot_id_).pose.position()).normalized(scaleFactor); - // point += last_world_state_->get_robot(true, robot_id_).pose.position(); + // rj_geometry::Point point = (last_world_state_->ball.position - + // last_world_state_->get_robot(true, + // robot_id_).pose.position()).normalized(scaleFactor); point += + // last_world_state_->get_robot(true, robot_id_).pose.position(); // planning::LinearMotionInstant target{point}; // SPDLOG_INFO("KICK KICK KICK"); 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}; + auto kick_cmd = + planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true}; intent.motion_command = kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; From 0f23e209c97c651b9910e838af532a323bac287a Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sun, 27 Oct 2024 21:15:20 -0400 Subject: [PATCH 11/18] changes --- .../planning/primitives/angle_planning.cpp | 32 +++++++++++-------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 991072338c6..e046d8583b2 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -5,29 +5,33 @@ namespace planning { void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, - const AngleFunction& angle_function, const RotationConstraints& constraints) { + const AngleFunction& angle_function, + const RotationConstraints& constraints) { const RJ::Time start_time = start_instant.stamp; if (trajectory->empty()) { trajectory->append_instant(start_instant); - double delta_angle = - angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr) - - start_instant.heading(); - double time = Trapezoidal::get_time(delta_angle, delta_angle, constraints.max_speed, - constraints.max_accel, 0, 0); + double delta_angle = angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr) - start_instant.heading(); + + if (delta_angle > M_PI) { + delta_angle -= 2 * M_PI; + } + + if (delta_angle < -M_PI) { + delta_angle += 2 * M_PI; + } + + double time = Trapezoidal::get_time(abs(delta_angle), abs(delta_angle), constraints.max_speed, constraints.max_accel, 0, 0); + SPDLOG_INFO("delta angle: {}", delta_angle); for (int i = 1; i < time / TIME_STEP; i++) { double pos_out = 0; double speed_out = 0; - Trapezoidal::trapezoidal_motion(delta_angle, constraints.max_speed, - constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, - speed_out); - trajectory->append_instant(RobotInstant{ - rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, - rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, - start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); - // SPDLOG_INFO("speed_out: {}", speed_out); + Trapezoidal::trapezoidal_motion(abs(delta_angle), constraints.max_speed, constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, speed_out); + pos_out *= std::signbit(delta_angle) ? -1 : 1; + trajectory->append_instant(RobotInstant{rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); + SPDLOG_INFO("pos_out: {}", pos_out); } trajectory->mark_angles_valid(); return; From aec07505a3ef24643373d8c35b37fae4f20d4ddb Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 27 Oct 2024 21:18:22 -0400 Subject: [PATCH 12/18] Fix Code Style On rotate-kick (#2303) automated style fixes Co-authored-by: Squid5678 --- .../planning/primitives/angle_planning.cpp | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index e046d8583b2..5b69624aa3f 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -5,14 +5,15 @@ namespace planning { void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, - const AngleFunction& angle_function, - const RotationConstraints& constraints) { + const AngleFunction& angle_function, const RotationConstraints& constraints) { const RJ::Time start_time = start_instant.stamp; if (trajectory->empty()) { trajectory->append_instant(start_instant); - double delta_angle = angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr) - start_instant.heading(); - + double delta_angle = + angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr) - + start_instant.heading(); + if (delta_angle > M_PI) { delta_angle -= 2 * M_PI; } @@ -21,16 +22,21 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, delta_angle += 2 * M_PI; } - double time = Trapezoidal::get_time(abs(delta_angle), abs(delta_angle), constraints.max_speed, constraints.max_accel, 0, 0); - + double time = Trapezoidal::get_time(abs(delta_angle), abs(delta_angle), + constraints.max_speed, constraints.max_accel, 0, 0); SPDLOG_INFO("delta angle: {}", delta_angle); for (int i = 1; i < time / TIME_STEP; i++) { double pos_out = 0; double speed_out = 0; - Trapezoidal::trapezoidal_motion(abs(delta_angle), constraints.max_speed, constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, speed_out); + Trapezoidal::trapezoidal_motion(abs(delta_angle), constraints.max_speed, + constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, + speed_out); pos_out *= std::signbit(delta_angle) ? -1 : 1; - trajectory->append_instant(RobotInstant{rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); + trajectory->append_instant(RobotInstant{ + rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, + rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, + start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); SPDLOG_INFO("pos_out: {}", pos_out); } trajectory->mark_angles_valid(); From 8f3e989f4871ffbd4c92ced1c01d7b3e747e127f Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sun, 27 Oct 2024 21:24:20 -0400 Subject: [PATCH 13/18] change to is angle done threshold --- soccer/src/soccer/planning/planner/rotate_path_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 924288ea539..fd40e199ee3 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -62,7 +62,7 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { Trajectory path{}; - if (abs(*cached_target_angle_ - target_angle) < degrees_to_radians(1)) { + if (abs(*cached_target_angle_ - target_angle) < degrees_to_radians(kIsDoneAngleChangeThresh)) { if (cached_path_) { path = cached_path_.value(); } else { From 52677834bb90fba34b7790286ca2da2ac1086dde Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sun, 27 Oct 2024 21:26:10 -0400 Subject: [PATCH 14/18] removing SPDLOG stuff --- soccer/src/soccer/planning/primitives/angle_planning.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 5b69624aa3f..82adb2b338b 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -25,7 +25,6 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, double time = Trapezoidal::get_time(abs(delta_angle), abs(delta_angle), constraints.max_speed, constraints.max_accel, 0, 0); - SPDLOG_INFO("delta angle: {}", delta_angle); for (int i = 1; i < time / TIME_STEP; i++) { double pos_out = 0; double speed_out = 0; @@ -37,7 +36,6 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); - SPDLOG_INFO("pos_out: {}", pos_out); } trajectory->mark_angles_valid(); return; From e83f57080a12ec6ff83add5eec7c61d00628a578 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Tue, 29 Oct 2024 19:44:42 -0400 Subject: [PATCH 15/18] removed unnecessary line pivot changes --- .../src/soccer/planning/planner/line_pivot_path_planner.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp b/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp index b76f31fefd2..b68e14adaf8 100644 --- a/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp @@ -22,8 +22,6 @@ Trajectory LinePivotPathPlanner::plan(const PlanRequest& request) { current_state_ = next_state(request); Trajectory path; - SPDLOG_INFO("Current state is {}", current_state_); - if (current_state_ == LINE) { path = line(request); } else { @@ -96,7 +94,7 @@ Trajectory LinePivotPathPlanner::pivot(const PlanRequest& request) { const MotionCommand& command = request.motion_command; - double radius = 0.1; // pivot::PARAM_radius_multiplier * command.pivot_radius; + double radius = pivot::PARAM_radius_multiplier * command.pivot_radius; auto pivot_point = command.pivot_point; auto pivot_target = command.target.position; From 5d602a2db1ec84f7a8800fdff5249cf3706b3fd2 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Tue, 29 Oct 2024 19:47:16 -0400 Subject: [PATCH 16/18] gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 176668b4431..0954366ef7e 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,7 @@ # build directories /build +/install /gmon.out *.swp /.settings From b3f6821f30a67176f38411228ee95df808c4559e Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Tue, 29 Oct 2024 19:50:46 -0400 Subject: [PATCH 17/18] remove log stuff --- .../src/soccer/planning/planner/rotate_path_planner.cpp | 9 --------- soccer/src/soccer/radio/sim_radio.cpp | 1 - 2 files changed, 10 deletions(-) diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index fd40e199ee3..93c2220bbbb 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -45,20 +45,13 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position(); auto pivot_target = command.target.position; - // SPDLOG_INFO("Pivot point x is {}", pivot_point.x()); - // SPDLOG_INFO("Pivot point y is {}", pivot_point.y()); - double start_angle = request.world_state->get_robot(true, static_cast(request.shell_id)).pose.heading(); double target_angle = pivot_point.angle_to(pivot_target); double angle_change = fix_angle_radians(target_angle - start_angle); - // SPDLOG_INFO("current angle: {}", start_angle); - // SPDLOG_INFO("cached_target_agne: {}", cached_target_angle_.value_or(0)); - // SPDLOG_INFO("target angle: {}", target_angle); cached_angle_change_ = angle_change; - // SPDLOG_INFO("cached angle change: {}", *cached_angle_change_); Trajectory path{}; @@ -66,14 +59,12 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { if (cached_path_) { path = cached_path_.value(); } else { - // SPDLOG_INFO("reset"); plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); path.stamp(RJ::now()); cached_path_ = path; } } else { - // SPDLOG_INFO("reset"); cached_path_.reset(); plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), request.constraints.rot); diff --git a/soccer/src/soccer/radio/sim_radio.cpp b/soccer/src/soccer/radio/sim_radio.cpp index 3375a947ab0..c0901018f56 100644 --- a/soccer/src/soccer/radio/sim_radio.cpp +++ b/soccer/src/soccer/radio/sim_radio.cpp @@ -88,7 +88,6 @@ SimRadio::SimRadio(bool blue_team) // file probably an issue with the hacky way I got param files to dynamically load std::string localhost = "127.0.0.1"; this->get_parameter_or("interface", param_radio_interface_, localhost); - // SPDLOG_INFO("SimRadio param_radio_interface_ {}", param_radio_interface_); address_ = boost::asio::ip::make_address(param_radio_interface_).to_v4(); robot_control_endpoint_ = ip::udp::endpoint(address_, blue_team_ ? kSimBlueCommandPort : kSimYellowCommandPort); From c49a4c3b668a2b27d28809ed34e65f3adc3d0fa7 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Tue, 29 Oct 2024 19:53:59 -0400 Subject: [PATCH 18/18] comments --- .../soccer/strategy/agent/position/solo_offense.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 7901189795e..3c45fd97374 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -51,11 +51,6 @@ SoloOffense::State SoloOffense::next_state() { return TO_BALL; } case TO_BALL: { - // SPDLOG_INFO("{}", (last_world_state_->get_robot(true, robot_id_).pose.position() - - // current_point).mag()); if ((last_world_state_->get_robot(true, - // robot_id_).pose.position() - current_point).mag() < kRobotDiameter){ - // return ROTATE; - // } if (check_is_done()) { return ROTATE; } @@ -70,10 +65,6 @@ SoloOffense::State SoloOffense::next_state() { return ROTATE; } case KICK: { - // if (check_is_done() || (last_world_state_->get_robot(true, robot_id_).pose.position() - // - current_point).mag() > kRobotDiameter * 3) { - // return TO_BALL; - // } if (!kick_ || (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point) .mag() > kRobotRadius * 5) { @@ -129,7 +120,6 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { // robot_id_).pose.position()).normalized(scaleFactor); point += // last_world_state_->get_robot(true, robot_id_).pose.position(); // planning::LinearMotionInstant target{point}; - // SPDLOG_INFO("KICK KICK KICK"); planning::LinearMotionInstant target{calculate_best_shot()}; // planning::LinearMotionInstant target{last_world_state_->ball.position}; auto kick_cmd =