From ec25fccca129e412d3214f07b048d8a8bf6e50bf Mon Sep 17 00:00:00 2001 From: sauravbanna Date: Thu, 20 Jun 2024 01:23:16 -0700 Subject: [PATCH 1/2] implemented chip modeling --- .../er_force_sim/src/amun/simulator/BUILD | 2 + .../src/amun/simulator/simball.cpp | 8 ++ src/proto/message_translation/BUILD | 1 + .../ssl_simulation_robot_control.cpp | 8 +- src/shared/constants.h | 1 + .../hl/stp/tactic/chip/chip_tactic_test.cpp | 66 ++++++----- src/software/ai/passing/BUILD | 34 +++++- src/software/ai/passing/base_pass.cpp | 49 ++++++++ src/software/ai/passing/base_pass.h | 109 ++++++++++++++++++ src/software/ai/passing/chip_pass.cpp | 63 ++++++++++ src/software/ai/passing/chip_pass.h | 38 ++++++ src/software/ai/passing/chip_pass_test.cpp | 12 ++ src/software/ai/passing/pass.cpp | 45 ++------ src/software/ai/passing/pass.h | 84 +------------- 14 files changed, 370 insertions(+), 150 deletions(-) create mode 100644 src/software/ai/passing/base_pass.cpp create mode 100644 src/software/ai/passing/base_pass.h create mode 100644 src/software/ai/passing/chip_pass.cpp create mode 100644 src/software/ai/passing/chip_pass.h create mode 100644 src/software/ai/passing/chip_pass_test.cpp diff --git a/src/extlibs/er_force_sim/src/amun/simulator/BUILD b/src/extlibs/er_force_sim/src/amun/simulator/BUILD index 76473358a4..a6d39bea5c 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/BUILD +++ b/src/extlibs/er_force_sim/src/amun/simulator/BUILD @@ -15,6 +15,8 @@ qt_cc_library( "@qt//:qt_core", "@qt//:qt_gui", "@qt//:qt_widgets", + "//software/logger:logger", + "//proto/message_translation:tbots_protobuf" ], #linkstatic = True, alwayslink = True, diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp index 054a1d8b24..ee84667433 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp @@ -28,6 +28,10 @@ #include "extlibs/er_force_sim/src/core/vector.h" #include "proto/ssl_vision_detection.pb.h" #include "simulator.h" +#include "software/logger/logger.h" +#include "proto/message_translation/tbots_protobuf.h" + +// #include "software/logger/logger.h" using namespace camun::simulator; @@ -82,6 +86,10 @@ void SimBall::begin(bool robot_collision) // custom implementation of rolling friction const btVector3 p = m_body->getWorldTransform().getOrigin(); const btVector3 velocity = m_body->getLinearVelocity(); + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"dist", std::sqrt(pow(p.x() - 0, 2) + pow(p.y() - 0, 2))}, + {"z", p.z()} + }); if (p.z() < static_cast(BALL_MAX_RADIUS_METERS) * 1.1f * SIMULATOR_SCALE) { // ball is on the ground bool is_stationary = diff --git a/src/proto/message_translation/BUILD b/src/proto/message_translation/BUILD index 8d7e578b09..801660ab38 100644 --- a/src/proto/message_translation/BUILD +++ b/src/proto/message_translation/BUILD @@ -162,6 +162,7 @@ cc_library( "//shared:robot_constants", "//software/geom:angle", "//software/logger", + "//proto/message_translation:tbots_protobuf" ], ) diff --git a/src/proto/message_translation/ssl_simulation_robot_control.cpp b/src/proto/message_translation/ssl_simulation_robot_control.cpp index fe87dde214..8a4d45329f 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control.cpp +++ b/src/proto/message_translation/ssl_simulation_robot_control.cpp @@ -3,6 +3,7 @@ #include "shared/constants.h" #include "software/geom/angle.h" #include "software/logger/logger.h" +#include "proto/message_translation/tbots_protobuf.h" // Converts rpm and wheel_radius_meters [m] to speed [m/s] float rpm_to_m_per_s(float rpm, float wheel_radius_meters) @@ -79,10 +80,13 @@ std::unique_ptr getRobotCommandFromDirectContr float numerator = range * static_cast(ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED); - float denominator = static_cast(2.0f * (chip_angle * 2.0f).sin()); + float denominator = static_cast(2.0f * (chip_angle * 2.0f).22()); float chip_speed = static_cast(std::sqrt(numerator / denominator)); kick_speed = chip_speed; + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"speed", chip_speed} + }); kick_angle = chip_angle.toDegrees(); break; } @@ -115,7 +119,7 @@ std::unique_ptr getRobotCommandFromDirectContr float numerator = range * static_cast( - ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED); + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED); float denominator = static_cast(2.0f * (chip_angle * 2.0f).sin()); float chip_speed = diff --git a/src/shared/constants.h b/src/shared/constants.h index 5ff6cc0b18..8bb50f414d 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -156,6 +156,7 @@ static const double MIN_CAPACITOR_VOLTAGE = 0; static const double MAX_CAPACITOR_VOLTAGE = 250.0 + 50.0; // +50v headroom static const unsigned int ROBOT_CHIP_ANGLE_DEGREES = 45; +static const double ROBOT_CHIP_ANGLE_RADIANS = ROBOT_CHIP_ANGLE_DEGREES * M_PI / 180; static const double CHICKER_TIMEOUT = 3 * MILLISECONDS_PER_SECOND; // How many robots are allowed in each division static const unsigned DIV_A_NUM_ROBOTS = 11; diff --git a/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp b/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp index 316c36c3d6..bdec712d37 100644 --- a/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp @@ -26,7 +26,7 @@ TEST_P(ChipTacticTest, chip_test) Vector ball_offset_from_robot = std::get<0>(GetParam()); Angle angle_to_kick_at = std::get<1>(GetParam()); - Point robot_position = Point(0, 0); + Point robot_position = Point(-0.5, 0); BallState ball_state(robot_position + ball_offset_from_robot, Vector(0, 0)); auto friendly_robots = @@ -35,47 +35,49 @@ TEST_P(ChipTacticTest, chip_test) auto tactic = std::make_shared(); tactic->updateControlParams(robot_position + ball_offset_from_robot, angle_to_kick_at, - 5); + 2); setTactic(1, tactic); - std::vector terminating_validation_functions = { - [angle_to_kick_at, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - while (!tactic->done()) - { - yield("Tactic did not complete!"); - } - ballKicked(angle_to_kick_at, world_ptr, yield); - }}; + // std::vector terminating_validation_functions = { + // [angle_to_kick_at, tactic](std::shared_ptr world_ptr, + // ValidationCoroutine::push_type& yield) { + // while (!tactic->done()) + // { + // yield("Tactic did not complete!"); + // } + // ballKicked(angle_to_kick_at, world_ptr, yield); + // }}; + std::vector terminating_validation_functions = {}; std::vector non_terminating_validation_functions = {}; runTest(field_type, ball_state, friendly_robots, enemy_robots, terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); + Duration::fromSeconds(15)); } INSTANTIATE_TEST_CASE_P( BallLocations, ChipTacticTest, ::testing::Values( // place the ball directly to the left of the robot - std::make_tuple(Vector(0, 0.5), Angle::zero()), - // place the ball directly to the right of the robot - std::make_tuple(Vector(0, -0.5), Angle::zero()), - // place the ball directly infront of the robot - std::make_tuple(Vector(0.5, 0), Angle::zero()), - // place the ball directly behind the robot - std::make_tuple(Vector(-0.5, 0), Angle::zero()), - // place the ball in the robots dribbler - std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()), - // Repeat the same tests but kick in the opposite direction - // place the ball directly to the left of the robot - std::make_tuple(Vector(0, 0.5), Angle::half()), - // place the ball directly to the right of the robot - std::make_tuple(Vector(0, -0.5), Angle::half()), - // place the ball directly infront of the robot - std::make_tuple(Vector(0.5, 0), Angle::half()), - // place the ball directly behind the robot - std::make_tuple(Vector(-0.5, 0), Angle::half()), - // place the ball in the robots dribbler - std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()))); + // std::make_tuple(Vector(0, 0.5), Angle::zero()), + // // place the ball directly to the right of the robot + // std::make_tuple(Vector(0, -0.5), Angle::zero()) + // // place the ball directly infront of the robot + std::make_tuple(Vector(0.5, 0), Angle::zero()) + // // place the ball directly behind the robot + // std::make_tuple(Vector(-0.5, 0), Angle::zero()), + // // place the ball in the robots dribbler + // std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()), + // // Repeat the same tests but kick in the opposite direction + // // place the ball directly to the left of the robot + // std::make_tuple(Vector(0, 0.5), Angle::half()), + // // place the ball directly to the right of the robot + // std::make_tuple(Vector(0, -0.5), Angle::half()), + // // place the ball directly infront of the robot + // std::make_tuple(Vector(0.5, 0), Angle::half()), + // // place the ball directly behind the robot + // std::make_tuple(Vector(-0.5, 0), Angle::half()), + // // place the ball in the robots dribbler + // std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()) + )); diff --git a/src/software/ai/passing/BUILD b/src/software/ai/passing/BUILD index 8671c4b2b2..0f732a0031 100644 --- a/src/software/ai/passing/BUILD +++ b/src/software/ai/passing/BUILD @@ -35,9 +35,29 @@ cc_library( srcs = ["pass.cpp"], hdrs = ["pass.h"], deps = [ + ":base_pass", "//shared:constants", - "//software/time:timestamp", - "//software/world:field", + "//software/time:timestamp" + ], +) + +cc_library( + name = "base_pass", + srcs = ["base_pass.cpp"], + hdrs = ["base_pass.h"], + deps = [ + "//software/geom:point", + "//software/time:duration" + ], +) + +cc_library( + name = "chip_pass", + srcs = ["chip_pass.cpp"], + hdrs = ["chip_pass.h"], + deps = [ + ":base_pass", + "//shared:constants" ], ) @@ -51,6 +71,16 @@ cc_test( ], ) +cc_test( + name = "chip_pass_test", + srcs = ["chip_pass_test.cpp"], + deps = [ + ":chip_pass", + "//shared/test_util:tbots_gtest_main", + "//software/test_util", + ], +) + cc_library( name = "pass_with_rating", srcs = ["pass_with_rating.cpp"], diff --git a/src/software/ai/passing/base_pass.cpp b/src/software/ai/passing/base_pass.cpp new file mode 100644 index 0000000000..443aac3510 --- /dev/null +++ b/src/software/ai/passing/base_pass.cpp @@ -0,0 +1,49 @@ +#include "software/ai/passing/base_pass.h" + +BasePass::BasePass(Point passer_point, Point receiver_point) + : passer_point(passer_point), + receiver_point(receiver_point) {} + +Point BasePass::receiverPoint() const +{ + return receiver_point; +} + +Angle BasePass::receiverOrientation() const +{ + return (passerPoint() - receiverPoint()).orientation(); +} + +Angle BasePass::passerOrientation() const +{ + return (receiverPoint() - passerPoint()).orientation(); +} + +Point BasePass::passerPoint() const +{ + return passer_point; +} + +double BasePass::length() const +{ + return std::sqrt(std::pow(receiverPoint().x() - passerPoint().x(), 2) + std::pow(receiverPoint().y() - passerPoint().y(), 2)); +} + +std::array BasePass::toPassArray() const +{ + return {receiver_point.x(), receiver_point.y()}; +} + +// std::ostream& printHelper(std::ostream& output_stream, const BasePass& pass) +// { +// output_stream << "Pass from " << pass.passer_point +// << " to: " << pass.receiver_point; + +// return output_stream; +// } + +bool BasePass::operator==(const BasePass& other) const +{ + return this->passer_point == other.passer_point && + this->receiver_point == other.receiver_point; +} \ No newline at end of file diff --git a/src/software/ai/passing/base_pass.h b/src/software/ai/passing/base_pass.h new file mode 100644 index 0000000000..49347e706a --- /dev/null +++ b/src/software/ai/passing/base_pass.h @@ -0,0 +1,109 @@ +#pragma once + +#include +#include +#include + +#include "software/geom/point.h" +#include "software/time/duration.h" + + +// The number of parameters (representing a pass) that we optimize +// (receive_location_x, receive_location_y) +static const int NUM_PARAMS_TO_OPTIMIZE = 2; + +class BasePass +{ + public: + BasePass() = delete; + + /** + * Gets the value of the passer point + * + * @return The value of the passer point + */ + Point passerPoint() const; + + /** + * Gets the value of the receiver point + * + * @return The value of the receiver point + */ + Point receiverPoint() const; + + /** + * Given the ball position, returns the angle the receiver should be + * facing to receive the pass. + * + * @return The angle the receiver should be facing + */ + Angle receiverOrientation() const; + + /** + * Given the ball position, returns the angle the passer should be + * facing to pass. + * + * @return The angle the passer should be facing + */ + Angle passerOrientation() const; + + /** + * Gets the length of the pass in metres + * + * @return The length of the pass in metres + */ + double length() const; + + /** + * Estimate how long the pass will take, from kicking to receiving + * + * This estimate does not account for friction on the ball + * + * @return An estimate of how long the pass will take, from kicking to receiving + */ + virtual Duration estimatePassDuration() const + { + return Duration::fromSeconds(0); + } + + /** + * Converts a pass to an array + * + * @returns the pass array: [receiver_point.x(), receiver_point.y()] + */ + std::array toPassArray() const; + + protected: + /** + * Create a pass with given parameters + * + * @param passer_point The point the pass should start at + * @param receiver_point The point the receiver should be at to receive the pass + */ + BasePass(Point passer_point, Point receiver_point); + + /** + // * Implement the "<<" operator for printing + // * + // * @param output_stream The stream to print to + // * @param pass The pass to print + // * @return The output stream with the string representation of the class appended + // */ + // friend std::ostream& printHelper(std::ostream& output_stream, const BasePass& pass); + + /** + * Compares Passes for equality. Passes are considered + * equal if all their member variables are equal. + * + * @param other the Pass to compare with for equality + * + * @return true if the Passes are equal and false otherwise + */ + virtual bool operator==(const BasePass& other) const; + + // The location of the passer + Point passer_point; + + // The location of the receiver + Point receiver_point; +}; \ No newline at end of file diff --git a/src/software/ai/passing/chip_pass.cpp b/src/software/ai/passing/chip_pass.cpp new file mode 100644 index 0000000000..55d849f3e2 --- /dev/null +++ b/src/software/ai/passing/chip_pass.cpp @@ -0,0 +1,63 @@ +#include "software/ai/passing/chip_pass.h" + + +ChipPass::ChipPass(Point passer_point, Point receiver_point) + : BasePass(passer_point, receiver_point) +{ + pass_length = length(); + first_bounce_range_m = calcFirstBounceRange(); +} + +double ChipPass::calcFirstBounceRange() +{ + double last_bounce_range; + double length_to_go = length(); + + std::cout << "LENGTH: " << length_to_go << std::endl; + + // if we cover more than 90% of the pass, stop calculating + while(length_to_go >= pass_length * 0.1) + { + double bounce_height = getBounceHeightFromDistanceTraveled(length_to_go); + last_bounce_range = getBounceRangeFromBounceHeight(bounce_height); + length_to_go -= last_bounce_range; + std::cout << "LENGTH: " << length_to_go << std::endl; + } + + return last_bounce_range; +} + +double ChipPass::getBounceHeightFromDistanceTraveled(double distance_traveled) +{ + return 0.0306 * std::exp(-2.04 * (distance_traveled - pass_length)); +} + +double ChipPass::getBounceRangeFromBounceHeight(double bounce_height) +{ + return 4 * bounce_height * (std::cos(ROBOT_CHIP_ANGLE_RADIANS) / std::sin(ROBOT_CHIP_ANGLE_RADIANS)); +} + +double ChipPass::firstBounceRange() +{ + return first_bounce_range_m; +} + +Duration ChipPass::estimatePassDuration() const +{ + return Duration::fromSeconds(0); +} + +std::ostream& operator<<(std::ostream& output_stream, const ChipPass& pass) +{ + output_stream << "Pass from " << pass.passer_point + << " to: " << pass.receiver_point + << " w/ First Bounce Range (m/s): " << pass.first_bounce_range_m; + + return output_stream; +} + +bool ChipPass::operator==(const ChipPass& other) const +{ + return BasePass::operator==(other) && + this->first_bounce_range_m == other.first_bounce_range_m; +} \ No newline at end of file diff --git a/src/software/ai/passing/chip_pass.h b/src/software/ai/passing/chip_pass.h new file mode 100644 index 0000000000..c17c570f30 --- /dev/null +++ b/src/software/ai/passing/chip_pass.h @@ -0,0 +1,38 @@ +#pragma once + +#include "software/ai/passing/base_pass.h" +#include "shared/constants.h" + +class ChipPass: public BasePass +{ + public: + ChipPass() = delete; + + /** + * Creates a chip pass from the given destination point + * + * @param passer_point the starting point of the pass + * @param pass_destination the end point of the pass + * @return the ChipPass constructed from the start and end points + */ + ChipPass(Point passer_point, Point receiver_point); + + double firstBounceRange(); + + virtual Duration estimatePassDuration() const; + + friend std::ostream& operator<<(std::ostream& output_stream, const ChipPass& pass); + + virtual bool operator==(const ChipPass& other) const; + + private: + + double calcFirstBounceRange(); + + double getBounceHeightFromDistanceTraveled(double distance_traveled); + + double getBounceRangeFromBounceHeight(double bounce_height); + + double first_bounce_range_m; + double pass_length; +}; \ No newline at end of file diff --git a/src/software/ai/passing/chip_pass_test.cpp b/src/software/ai/passing/chip_pass_test.cpp new file mode 100644 index 0000000000..3be05a2678 --- /dev/null +++ b/src/software/ai/passing/chip_pass_test.cpp @@ -0,0 +1,12 @@ +#include "software/ai/passing/chip_pass.h" + +#include + +TEST(PassTest, simple_getters) +{ + ChipPass p(Point(1, 2), Point(3, 4)); + + EXPECT_EQ(Point(1, 2), p.passerPoint()); + EXPECT_EQ(Point(3, 4), p.receiverPoint()); + std::cout << p.firstBounceRange() << std::endl; +} \ No newline at end of file diff --git a/src/software/ai/passing/pass.cpp b/src/software/ai/passing/pass.cpp index 654db10ca0..84f374de39 100644 --- a/src/software/ai/passing/pass.cpp +++ b/src/software/ai/passing/pass.cpp @@ -2,8 +2,7 @@ Pass::Pass(Point passer_point, Point receiver_point, double pass_speed_m_per_s) - : passer_point(passer_point), - receiver_point(receiver_point), + : BasePass(passer_point, receiver_point), pass_speed_m_per_s(pass_speed_m_per_s) { if (pass_speed_m_per_s < 0.0) @@ -12,32 +11,6 @@ Pass::Pass(Point passer_point, Point receiver_point, double pass_speed_m_per_s) } } -Point Pass::receiverPoint() const -{ - return receiver_point; -} - -Angle Pass::receiverOrientation() const -{ - return (passerPoint() - receiverPoint()).orientation(); -} - -Angle Pass::passerOrientation() const -{ - return (receiverPoint() - passerPoint()).orientation(); -} - - -Point Pass::passerPoint() const -{ - return passer_point; -} - -double Pass::speed() const -{ - return pass_speed_m_per_s; -} - Pass Pass::fromPassArray(Point passer_point, const std::array& array, double pass_speed_m_per_s) @@ -57,6 +30,11 @@ Pass Pass::fromDestReceiveSpeed(const Point& passer_point, const Point& receiver return Pass(passer_point, receiver_point, pass_speed_m_per_s); } +double Pass::speed() const +{ + return pass_speed_m_per_s; +} + double Pass::getPassSpeed(const Point& ball_position, const Point& pass_destination, double dest_speed_m_per_s, double min_pass_speed_m_per_s, double max_pass_speed_m_per_s) @@ -111,11 +89,6 @@ double Pass::getPassSpeed(const Point& ball_position, const Point& pass_destinat return clamped_pass_speed_m_per_s; } -std::array Pass::toPassArray() const -{ - return {receiver_point.x(), receiver_point.y()}; -} - Duration Pass::estimatePassDuration() const { return Duration::fromSeconds((receiver_point - passer_point).length() / @@ -124,7 +97,8 @@ Duration Pass::estimatePassDuration() const std::ostream& operator<<(std::ostream& output_stream, const Pass& pass) { - output_stream << "Receiver Point: " << pass.receiver_point + output_stream << "Pass from " << pass.passer_point + << " to: " << pass.receiver_point << " w/ Speed (m/s): " << pass.pass_speed_m_per_s; return output_stream; @@ -132,7 +106,6 @@ std::ostream& operator<<(std::ostream& output_stream, const Pass& pass) bool Pass::operator==(const Pass& other) const { - return this->passer_point == other.passer_point && - this->receiver_point == other.receiver_point && + return BasePass::operator==(other) && this->pass_speed_m_per_s == other.pass_speed_m_per_s; } diff --git a/src/software/ai/passing/pass.h b/src/software/ai/passing/pass.h index 148f1e2fc6..b7849b1206 100644 --- a/src/software/ai/passing/pass.h +++ b/src/software/ai/passing/pass.h @@ -1,21 +1,13 @@ #pragma once -#include -#include -#include - #include "shared/constants.h" -#include "software/geom/point.h" #include "software/time/timestamp.h" - -// The number of parameters (representing a pass) that we optimize -// (receive_location_x, receive_location_y) -static const int NUM_PARAMS_TO_OPTIMIZE = 2; +#include "software/ai/passing/base_pass.h" /** * This class represents a Pass, a receive point with a speed */ -class Pass +class Pass: public BasePass { public: Pass() = delete; @@ -76,84 +68,20 @@ class Pass double min_pass_speed_m_per_s, double max_pass_speed_m_per_s); - /** - * Converts a pass to an array - * - * @returns the pass array: [receiver_point.x(), receiver_point.y()] - */ - std::array toPassArray() const; - - /** - * Gets the value of the passer point - * - * @return The value of the passer point - */ - Point passerPoint() const; - - /** - * Gets the value of the receiver point - * - * @return The value of the receiver point - */ - Point receiverPoint() const; - - /** - * Given the ball position, returns the angle the receiver should be - * facing to receive the pass. - * - * @return The angle the receiver should be facing - */ - Angle receiverOrientation() const; - - /** - * Given the ball position, returns the angle the passer should be - * facing to pass. - * - * @return The angle the passer should be facing - */ - Angle passerOrientation() const; - /** * Gets the value of the pass speed * * @return The value of the pass speed, in meters/second */ - double speed() const; + double speed() const; - /** - * Estimate how long the pass will take, from kicking to receiving - * - * This estimate does not account for friction on the ball - * - * @return An estimate of how long the pass will take, from kicking to receiving - */ - Duration estimatePassDuration() const; + virtual Duration estimatePassDuration() const; - /** - * Implement the "<<" operator for printing - * - * @param output_stream The stream to print to - * @param pass The pass to print - * @return The output stream with the string representation of the class appended - */ friend std::ostream& operator<<(std::ostream& output_stream, const Pass& pass); - /** - * Compares Passes for equality. Passes are considered - * equal if all their member variables are equal. - * - * @param other the Pass to compare with for equality - * - * @return true if the Passes are equal and false otherwise - */ - bool operator==(const Pass& other) const; - - private: - // The location of the passer - Point passer_point; + virtual bool operator==(const Pass& other) const; - // The location of the receiver - Point receiver_point; + private: // The speed of the pass in meters/second double pass_speed_m_per_s; From 1e1065961b861b2440485048614d9e4500c7e5ec Mon Sep 17 00:00:00 2001 From: sauravbanna Date: Mon, 15 Jul 2024 00:53:02 -0700 Subject: [PATCH 2/2] changes --- .../message_translation/tbots_protobuf.cpp | 31 +++++-- .../message_translation/tbots_protobuf.h | 2 + src/proto/visualization.proto | 20 +++-- src/proto/world.proto | 19 +++- .../stp/tactic/attacker/attacker_tactic.cpp | 29 +++++-- src/software/ai/passing/BUILD | 6 +- src/software/ai/passing/base_pass.cpp | 8 -- src/software/ai/passing/base_pass.h | 22 ++--- src/software/ai/passing/chip_pass.cpp | 52 ++++++++++- src/software/ai/passing/chip_pass.h | 10 ++- src/software/ai/passing/chip_pass_test.cpp | 28 +++--- src/software/ai/passing/cost_function.cpp | 26 ++++-- src/software/ai/passing/cost_function.h | 5 +- src/software/ai/passing/pass.cpp | 7 +- src/software/ai/passing/pass.h | 2 + src/software/ai/passing/pass_generator.hpp | 87 ++++++++++++++----- src/software/ai/passing/pass_with_rating.cpp | 3 +- src/software/ai/passing/pass_with_rating.h | 4 +- 18 files changed, 260 insertions(+), 101 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index bc5623b8c5..aedf7c954d 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -376,17 +376,32 @@ std::unique_ptr createPassVisualization( for (const auto& pass_with_rating : passes_with_rating) { - auto pass_msg = std::make_unique(); - *(pass_msg->mutable_passer_point()) = - *createPointProto(pass_with_rating.pass.passerPoint()); - *(pass_msg->mutable_receiver_point()) = - *createPointProto(pass_with_rating.pass.receiverPoint()); - pass_msg->set_pass_speed_m_per_s(pass_with_rating.pass.speed()); - auto pass_with_rating_msg = std::make_unique(); pass_with_rating_msg->set_rating(pass_with_rating.rating); - *(pass_with_rating_msg->mutable_pass_()) = *pass_msg; + auto pass = pass_with_rating.pass; + + auto base_pass_msg = std::make_unique(); + *(base_pass_msg->mutable_passer_point()) = + *createPointProto(pass.passerPoint()); + *(base_pass_msg->mutable_receiver_point()) = + *createPointProto(pass.receiverPoint()); + + if (pass.type() == PassType::CHIP_PASS) + { + auto chip_pass_msg = std::make_unique(); + *(chip_pass_msg->mutable_pass_coords()) = *base_pass_msg; + chip_pass_msg->set_chip_distance_meters(reinterpret_cast(&pass)->firstBounceRange()); + *(pass_with_rating_msg->mutable_pass_()->mutable_chip_pass()) = *chip_pass_msg; + } + else + { + auto pass_msg = std::make_unique(); + *(pass_msg->mutable_pass_coords()) = *base_pass_msg; + pass_msg->set_pass_speed_m_per_s(reinterpret_cast(&pass)->speed()); + *(pass_with_rating_msg->mutable_pass_()->mutable_ground_pass()) = *pass_msg; + } + *(pass_visualization_msg->add_best_passes()) = *pass_with_rating_msg; } return pass_visualization_msg; diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 15f8cef4ed..b07c478dfb 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -8,6 +8,8 @@ #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/ai/navigator/trajectory/trajectory_path.h" #include "software/ai/passing/pass_with_rating.h" +#include "software/ai/passing/pass.h" +#include "software/ai/passing/chip_pass.h" #include "software/world/world.h" /** diff --git a/src/proto/visualization.proto b/src/proto/visualization.proto index 2dc1165ac2..a936569183 100644 --- a/src/proto/visualization.proto +++ b/src/proto/visualization.proto @@ -24,8 +24,12 @@ message PlotJugglerValue message PassWithRating { - double rating = 1; - Pass pass_ = 2; // needs the _ because pass is a keyword in python + double rating = 1; + oneof pass_ // needs the _ because pass is a keyword in python + { + Pass ground_pass = 2; + ChipPass chip_pass = 3; + } } message PassVisualization @@ -35,10 +39,14 @@ message PassVisualization message AttackerVisualization { - optional Pass pass_ = 1; // needs the _ because pass is a keyword in python - bool pass_committed = 2; - optional Shot shot = 3; - optional Point chip_target = 4; + oneof pass_ // needs the _ because pass is a keyword in python + { + Pass ground_pass = 1; + ChipPass chip_pass = 2; + } + bool pass_committed = 3; + optional Shot shot = 4; + optional Point chip_target = 5; } message CostVisualization diff --git a/src/proto/world.proto b/src/proto/world.proto index 07eaef87db..4eb243cfab 100644 --- a/src/proto/world.proto +++ b/src/proto/world.proto @@ -60,16 +60,31 @@ message SimulationState required double simulation_speed = 2 [default = 1.0]; } -message Pass +message BasePass { // The location of the passer required Point passer_point = 1; // The location of the receiver required Point receiver_point = 2; +} + +message Pass +{ + // The pass coordinates + required BasePass pass_coords = 1; // The speed of the pass in meters/second - required double pass_speed_m_per_s = 3; + required double pass_speed_m_per_s = 2; +} + +message ChipPass +{ + // The pass coordinates + required BasePass pass_coords; + + // The range of the first bounce of the chip + required double chip_distance_meters = 3; } message Shot diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp index 575b284f13..7f920bb168 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp @@ -79,13 +79,28 @@ void AttackerTactic::visualizeControlParams( if (control_params.best_pass_so_far.has_value()) { - TbotsProto::Pass pass_msg; - *(pass_msg.mutable_passer_point()) = - *createPointProto(control_params.best_pass_so_far->passerPoint()); - *(pass_msg.mutable_receiver_point()) = - *createPointProto(control_params.best_pass_so_far->receiverPoint()); - pass_msg.set_pass_speed_m_per_s(control_params.best_pass_so_far->speed()); - *(pass_visualization_msg.mutable_pass_()) = pass_msg; + auto pass = control_params.best_pass_so_far; + + auto base_pass_msg = std::make_unique(); + *(base_pass_msg->mutable_passer_point()) = + *createPointProto(pass.passerPoint()); + *(base_pass_msg->mutable_receiver_point()) = + *createPointProto(pass.receiverPoint()); + + if (pass.type() == PassType::CHIP_PASS) + { + auto chip_pass_msg = std::make_unique(); + *(chip_pass_msg->mutable_pass_coords()) = *base_pass_msg; + chip_pass_msg->set_chip_distance_meters(reinterpret_cast(&pass)->firstBounceRange()); + *(pass_visualization_msg->mutable_pass_()->mutable_chip_pass()) = *chip_pass_msg; + } + else + { + auto pass_msg = std::make_unique(); + *(pass_msg->mutable_pass_coords()) = *base_pass_msg; + pass_msg->set_pass_speed_m_per_s(reinterpret_cast(&pass)->speed()); + *(pass_visualization_msg->mutable_pass_()->mutable_ground_pass()) = *pass_msg; + } } pass_visualization_msg.set_pass_committed(pass_committed); diff --git a/src/software/ai/passing/BUILD b/src/software/ai/passing/BUILD index 0f732a0031..53669ff6f9 100644 --- a/src/software/ai/passing/BUILD +++ b/src/software/ai/passing/BUILD @@ -8,6 +8,7 @@ cc_library( hdrs = ["cost_function.h"], deps = [ ":pass", + ":chip_pass", "//proto/message_translation:tbots_protobuf", "//software/ai/evaluation:calc_best_shot", "//software/ai/evaluation:time_to_travel", @@ -57,7 +58,8 @@ cc_library( hdrs = ["chip_pass.h"], deps = [ ":base_pass", - "//shared:constants" + "//shared:constants", + "//software/geom/algorithms" ], ) @@ -86,7 +88,7 @@ cc_library( srcs = ["pass_with_rating.cpp"], hdrs = ["pass_with_rating.h"], deps = [ - ":pass", + ":base_pass", ], ) diff --git a/src/software/ai/passing/base_pass.cpp b/src/software/ai/passing/base_pass.cpp index 443aac3510..cfc3f3ebe4 100644 --- a/src/software/ai/passing/base_pass.cpp +++ b/src/software/ai/passing/base_pass.cpp @@ -34,14 +34,6 @@ std::array BasePass::toPassArray() const return {receiver_point.x(), receiver_point.y()}; } -// std::ostream& printHelper(std::ostream& output_stream, const BasePass& pass) -// { -// output_stream << "Pass from " << pass.passer_point -// << " to: " << pass.receiver_point; - -// return output_stream; -// } - bool BasePass::operator==(const BasePass& other) const { return this->passer_point == other.passer_point && diff --git a/src/software/ai/passing/base_pass.h b/src/software/ai/passing/base_pass.h index 90522e1d02..62c21fa03c 100644 --- a/src/software/ai/passing/base_pass.h +++ b/src/software/ai/passing/base_pass.h @@ -6,7 +6,10 @@ #include "software/geom/point.h" #include "software/time/duration.h" +#include "software/util/make_enum/make_enum.h" +MAKE_ENUM(PassType, + BASE_PASS, GROUND_PASS, CHIP_PASS); // The number of parameters (representing a pass) that we optimize // (receive_location_x, receive_location_y) @@ -66,7 +69,15 @@ class BasePass return Duration::fromSeconds(0); } - virtual Duration estimateTimeToPoint(Point& point) const; + virtual Duration estimateTimeToPoint(Point& point) const + { + return Duration::fromSeconds(0); + } + + virtual PassType type() const + { + return PassType::BASE_PASS; + } /** * Converts a pass to an array @@ -84,15 +95,6 @@ class BasePass */ BasePass(Point passer_point, Point receiver_point); - /** - // * Implement the "<<" operator for printing - // * - // * @param output_stream The stream to print to - // * @param pass The pass to print - // * @return The output stream with the string representation of the class appended - // */ - // friend std::ostream& printHelper(std::ostream& output_stream, const BasePass& pass); - /** * Compares Passes for equality. Passes are considered * equal if all their member variables are equal. diff --git a/src/software/ai/passing/chip_pass.cpp b/src/software/ai/passing/chip_pass.cpp index 901a502853..cf3249696f 100644 --- a/src/software/ai/passing/chip_pass.cpp +++ b/src/software/ai/passing/chip_pass.cpp @@ -1,12 +1,49 @@ #include "software/ai/passing/chip_pass.h" - ChipPass::ChipPass(Point passer_point, Point receiver_point) : BasePass(passer_point, receiver_point), - bounce_ranges() + skip_area({Point(0, 0), Point(0, 0), Point(0, 0), Point(0, 0)}) { pass_length = length(); first_bounce_range_m = calcFirstBounceRange(); + skip_area = calcSkipArea(); + std::cout << "SKIP: " << skip_area << std::endl; +} + +bool ChipPass::isSkipped(const Point& point) const +{ + return contains(skip_area, point); +} + +Polygon ChipPass::calcSkipArea() +{ + std::vector>pass_info (bounce_heights_and_ranges.rbegin(), bounce_heights_and_ranges.rend()); + double distance_until_intercept = 0; + for (auto &height_and_range: pass_info) + { + std::cout << "HEIGHT: " << height_and_range.first << " RANGE: " << height_and_range.second << std::endl; + if (height_and_range.first < ROBOT_MAX_HEIGHT_METERS) + { + break; + } + + distance_until_intercept += height_and_range.second; + } + + Vector pass_direction(receiver_point.x() - passer_point.x(), receiver_point.y() - passer_point.y()); + + Vector vec_on_pass = pass_direction.normalize(distance_until_intercept); + Point point_on_pass(passer_point.x() + vec_on_pass.x(), passer_point.y() + vec_on_pass.y()); + + Vector perpendicular = pass_direction.perpendicular().normalize(1.5); + Vector perpendicular_opp = perpendicular.rotate(Angle::fromDegrees(180)); + + return Polygon({ + Point(passer_point.x() + perpendicular.x(), passer_point.y() + perpendicular.y()), + Point(passer_point.x() + perpendicular_opp.x(), passer_point.y() + perpendicular_opp.y()), + Point(point_on_pass.x() + perpendicular.x(), point_on_pass.y() + perpendicular.y()), + Point(point_on_pass.x() + perpendicular_opp.x(), point_on_pass.y() + perpendicular_opp.y()) + }); } double ChipPass::calcFirstBounceRange() @@ -19,7 +56,9 @@ double ChipPass::calcFirstBounceRange() { double bounce_height = getBounceHeightFromDistanceTraveled(length_to_go); last_bounce_range = getBounceRangeFromBounceHeight(bounce_height); - bounce_ranges.push_back(last_bounce_range); + bounce_heights_and_ranges.push_back(std::make_pair(bounce_height, last_bounce_range)); + + std::cout << "HEIGHT: " << bounce_height << " RANGE: " << last_bounce_range << std::endl; length_to_go -= last_bounce_range; } @@ -51,9 +90,14 @@ Duration ChipPass::estimateTimeToPoint(Point& point) const return Duration::fromSeconds(0); } +PassType Chipass::type() const +{ + return PassType::CHIP_PASS; +} + std::ostream& operator<<(std::ostream& output_stream, const ChipPass& pass) { - output_stream << "Pass from " << pass.passer_point + output_stream << "Pass from " << pass.passer_point << " to: " << pass.receiver_point << " w/ First Bounce Range (m/s): " << pass.first_bounce_range_m; diff --git a/src/software/ai/passing/chip_pass.h b/src/software/ai/passing/chip_pass.h index dbf4587d88..b6fed18d46 100644 --- a/src/software/ai/passing/chip_pass.h +++ b/src/software/ai/passing/chip_pass.h @@ -2,6 +2,7 @@ #include "software/ai/passing/base_pass.h" #include "shared/constants.h" +#include "software/geom/algorithms/contains.h" class ChipPass: public BasePass { @@ -19,6 +20,8 @@ class ChipPass: public BasePass double firstBounceRange(); + bool isSkipped(const Point& point) const; + virtual Duration estimatePassDuration() const; virtual Duration estimateTimeToPoint(Point& point) const; @@ -27,15 +30,20 @@ class ChipPass: public BasePass virtual bool operator==(const ChipPass& other) const; + virtual PassType type() const; + private: double calcFirstBounceRange(); + Polygon calcSkipArea(); + double getBounceHeightFromDistanceTraveled(double distance_traveled); double getBounceRangeFromBounceHeight(double bounce_height); double first_bounce_range_m; double pass_length; - std::vector bounce_ranges; + std::vector> bounce_heights_and_ranges; + Polygon skip_area; }; \ No newline at end of file diff --git a/src/software/ai/passing/chip_pass_test.cpp b/src/software/ai/passing/chip_pass_test.cpp index 1d8b75e902..0f9afbfd5a 100644 --- a/src/software/ai/passing/chip_pass_test.cpp +++ b/src/software/ai/passing/chip_pass_test.cpp @@ -11,20 +11,20 @@ TEST(PassTest, simple_getters) std::cout << p.firstBounceRange() << std::endl; } -TEST(PassTest2, simple_getters) -{ - ChipPass p(Point(0, 0), Point(2, 0)); +// TEST(PassTest2, simple_getters) +// { +// ChipPass p(Point(0, 0), Point(2, 0)); - EXPECT_EQ(Point(0, 0), p.passerPoint()); - EXPECT_EQ(Point(2, 0), p.receiverPoint()); - std::cout << p.firstBounceRange() << std::endl; -} +// EXPECT_EQ(Point(0, 0), p.passerPoint()); +// EXPECT_EQ(Point(2, 0), p.receiverPoint()); +// std::cout << p.firstBounceRange() << std::endl; +// } -TEST(PassTest3, simple_getters) -{ - ChipPass p(Point(0, 0), Point(3, 0)); +// TEST(PassTest3, simple_getters) +// { +// ChipPass p(Point(0, 0), Point(3, 0)); - EXPECT_EQ(Point(0, 0), p.passerPoint()); - EXPECT_EQ(Point(3, 0), p.receiverPoint()); - std::cout << p.firstBounceRange() << std::endl; -} \ No newline at end of file +// EXPECT_EQ(Point(0, 0), p.passerPoint()); +// EXPECT_EQ(Point(3, 0), p.receiverPoint()); +// std::cout << p.firstBounceRange() << std::endl; +// } \ No newline at end of file diff --git a/src/software/ai/passing/cost_function.cpp b/src/software/ai/passing/cost_function.cpp index bd4da426f0..e4087fd73a 100644 --- a/src/software/ai/passing/cost_function.cpp +++ b/src/software/ai/passing/cost_function.cpp @@ -13,16 +13,28 @@ #include "software/geom/algorithms/convex_angle.h" #include "software/logger/logger.h" -double rateGroundPass(const World& world, const Pass& pass, const Rectangle& zone, +double rateAnyPass(const World& world, const BasePass& pass, const Rectangle& zone, TbotsProto::PassingConfig passing_config) { - return ratePass(world, pass, zone, world.enemyTeam(), passing_config); -} + if (pass.type() == PassType::CHIP_PASS) + { + std::vector enemy_team_not_skipped; -double rateChipPass(const World& world, const ChipPass& pass, const Rectangle& zone, - TbotsProto::PassingConfig passing_config) -{ - return ratePass(world, pass, zone, world.enemyTeam(), passing_config); + for (auto &enemy_robot : world.enemyTeam().getAllRobots()) + { + if (!reinterpret_cast(&pass)->isSkipped(enemy_robot.position())) + { + enemy_team_not_skipped.push_back(enemy_robot); + } + } + return ratePass(world, pass, zone, Team(enemy_team_not_skipped), passing_config); + } + else if (pass.type() == PassType::GROUND_PASS) + { + return ratePass(world, pass, zone, world.enemyTeam(), passing_config); + } + + return 0.0; } double ratePass(const World& world, const BasePass& pass, const Rectangle& zone, const Team& enemy_team, diff --git a/src/software/ai/passing/cost_function.h b/src/software/ai/passing/cost_function.h index a7f38d4495..0d6391f099 100644 --- a/src/software/ai/passing/cost_function.h +++ b/src/software/ai/passing/cost_function.h @@ -5,17 +5,16 @@ #include "proto/message_translation/tbots_protobuf.h" #include "proto/parameters.pb.h" #include "software/ai/passing/pass.h" +#include "software/ai/passing/chip_pass.h" #include "software/math/math_functions.h" #include "software/util/make_enum/make_enum.h" #include "software/world/field.h" #include "software/world/team.h" #include "software/world/world.h" -double rateGroundPass(const World& world, const Pass& pass, const Rectangle& zone, +double ratePass(const World& world, const BasePass& pass, const Rectangle& zone, TbotsProto::PassingConfig passing_config); -double rateChipPass(const World& world, const ChipPass& pass, const Rectangle& zone, - TbotsProto::PassingConfig passing_config) /** * Calculate the quality of a given pass * diff --git a/src/software/ai/passing/pass.cpp b/src/software/ai/passing/pass.cpp index 5cd2361908..8c1a3ac114 100644 --- a/src/software/ai/passing/pass.cpp +++ b/src/software/ai/passing/pass.cpp @@ -100,9 +100,14 @@ Duration Pass::estimateTimeToPoint(Point& point) const return Duration::fromSeconds(0); } +PassType Pass::type() const +{ + return PassType::GROUND_PASS; +} + std::ostream& operator<<(std::ostream& output_stream, const Pass& pass) { - output_stream << "Pass from " << pass.passer_point + output_stream << "Pass from " << pass.passer_point << " to: " << pass.receiver_point << " w/ Speed (m/s): " << pass.pass_speed_m_per_s; diff --git a/src/software/ai/passing/pass.h b/src/software/ai/passing/pass.h index 6e3c9d8007..df873b5d7f 100644 --- a/src/software/ai/passing/pass.h +++ b/src/software/ai/passing/pass.h @@ -79,6 +79,8 @@ class Pass: public BasePass virtual Duration estimateTimeToPoint(Point& point) const; + virtual PassType type() const; + friend std::ostream& operator<<(std::ostream& output_stream, const Pass& pass); virtual bool operator==(const Pass& other) const; diff --git a/src/software/ai/passing/pass_generator.hpp b/src/software/ai/passing/pass_generator.hpp index 95fedf7428..7a8044f995 100644 --- a/src/software/ai/passing/pass_generator.hpp +++ b/src/software/ai/passing/pass_generator.hpp @@ -185,15 +185,24 @@ ZonePassMap PassGenerator::samplePasses(const World& world) auto pass_destination = Point(x_distribution(random_num_gen_), y_distribution(random_num_gen_)); - auto pass = Pass::fromDestReceiveSpeed(world.ball().position(), pass_destination, + auto ground_pass = Pass::fromDestReceiveSpeed(world.ball().position(), pass_destination, passing_config_.max_receive_speed(), passing_config_.min_pass_speed_m_per_s(), passing_config_.max_pass_speed_m_per_s()); + auto ground_pass_rating = ratePass(world, ground_pass, pitch_division_->getZone(zone_id), passing_config_); - passes.emplace( - zone_id, - PassWithRating{pass, ratePass(world, pass, pitch_division_->getZone(zone_id), - passing_config_)}); + auto chip_pass = ChipPass(world.ball().position(), pass_destination); + auto chip_pass_rating = ratePass(world, chip_pass, pitch_division_->getZone(zone_id), passing_config_); + + if (chip_pass_rating > ground_pass_rating) + { + passes.emplace(zone_id, PassWithRating{chip_pass, chip_pass_rating}); + } + else + { + passes.emplace(zone_id, PassWithRating{ground_pass, ground_pass_rating}); + } + } return passes; @@ -211,10 +220,10 @@ ZonePassMap PassGenerator::optimizePasses( { // The objective function we minimize in gradient descent to improve each pass // that we're optimizing - const auto objective_function = + const auto ground_pass_objective_function = [this, &world, zone_id](const std::array& pass_array) { - // get a pass with the new appropriate speed using the new destination + // get a ground pass with the new appropriate speed using the new destination return ratePass( world, Pass::fromDestReceiveSpeed(world.ball().position(), @@ -225,20 +234,42 @@ ZonePassMap PassGenerator::optimizePasses( pitch_division_->getZone(zone_id), passing_config_); }; - auto optimized_pass_array = optimizer_.maximize( - objective_function, generated_passes.at(zone_id).pass.toPassArray(), + auto optimized_ground_pass_array = optimizer_.maximize( + ground_pass_objective_function, generated_passes.at(zone_id).pass.toPassArray(), passing_config_.number_of_gradient_descent_steps_per_iter()); // get a pass with the new appropriate speed using the new destination - auto new_pass = Pass::fromDestReceiveSpeed( - world.ball().position(), - Point(optimized_pass_array[0], optimized_pass_array[1]), - passing_config_.max_receive_speed(), passing_config_.min_pass_speed_m_per_s(), - passing_config_.max_pass_speed_m_per_s()); - auto score = - ratePass(world, new_pass, pitch_division_->getZone(zone_id), passing_config_); - - optimized_passes.emplace(zone_id, PassWithRating{new_pass, score}); + auto ground_pass = Pass::fromDestReceiveSpeed(world.ball().position(), Point(optimized_ground_pass_array[0], optimized_ground_pass_array[1]), + passing_config_.max_receive_speed(), + passing_config_.min_pass_speed_m_per_s(), + passing_config_.max_pass_speed_m_per_s()); + auto ground_pass_rating = ratePass(world, ground_pass, pitch_division_->getZone(zone_id), passing_config_); + + const auto chip_pass_objective_function = + [this, &world, + zone_id](const std::array& pass_array) { + // get a chip pass using the new destination + return ratePass( + world, + ChipPass(world.ball().position(), Point(pass_array[0], pass_array[1])), + pitch_division_->getZone(zone_id), passing_config_); + }; + + auto optimized_chip_pass_array = optimizer_.maximize( + chip_pass_objective_function, generated_passes.at(zone_id).pass.toPassArray(), + passing_config_.number_of_gradient_descent_steps_per_iter()); + + auto chip_pass = ChipPass(world.ball().position(), Point(optimized_chip_pass_array[0], optimized_chip_pass_array[1])); + auto chip_pass_rating = ratePass(world, chip_pass, pitch_division_->getZone(zone_id), passing_config_); + + if (chip_pass_rating > ground_pass_rating) + { + optimized_passes.emplace(zone_id, PassWithRating{chip_pass, chip_pass_rating}); + } + else + { + optimized_passes.emplace(zone_id, PassWithRating{ground_pass, ground_pass_rating}); + } } return optimized_passes; @@ -252,12 +283,20 @@ void PassGenerator::updatePasses(const World& world, { for (ZoneEnum zone_id : pitch_division_->getAllZoneIds()) { - auto pass_array = current_best_passes_.at(zone_id).pass.toPassArray(); - // update the passer point of the current best pass - current_best_passes_.at(zone_id).pass = Pass::fromDestReceiveSpeed( - world.ball().position(), Point(pass_array[0], pass_array[1]), - passing_config_.max_receive_speed(), passing_config_.min_pass_speed_m_per_s(), - passing_config_.max_pass_speed_m_per_s()); + auto current_best_pass = current_best_passes_.at(zone_id).pass; + + if (current_best_pass.type() == PassType::CHIP_PASS) + { + current_best_passes_.at(zone_id).pass = ChipPass( + world.ball().position(), current_best_pass.receiverPoint()); + } + else + { + current_best_passes_.at(zone_id).pass = Pass::fromDestReceiveSpeed( + world.ball().position(), current_best_pass.receiverPoint(), + passing_config_.max_receive_speed(), passing_config_.min_pass_speed_m_per_s(), + passing_config_.max_pass_speed_m_per_s()); + } if (ratePass(world, current_best_passes_.at(zone_id).pass, pitch_division_->getZone(zone_id), diff --git a/src/software/ai/passing/pass_with_rating.cpp b/src/software/ai/passing/pass_with_rating.cpp index c0702ba6cb..86b03ddcc5 100644 --- a/src/software/ai/passing/pass_with_rating.cpp +++ b/src/software/ai/passing/pass_with_rating.cpp @@ -3,6 +3,5 @@ bool operator==(const PassWithRating &lhs, const PassWithRating &rhs) { return lhs.rating == rhs.rating && - lhs.pass.receiverPoint() == rhs.pass.receiverPoint() && - lhs.pass.speed() == rhs.pass.speed(); + lhs.pass.receiverPoint() == rhs.pass.receiverPoint(); } diff --git a/src/software/ai/passing/pass_with_rating.h b/src/software/ai/passing/pass_with_rating.h index 2b8bd8b181..7cd9692ddf 100644 --- a/src/software/ai/passing/pass_with_rating.h +++ b/src/software/ai/passing/pass_with_rating.h @@ -1,10 +1,10 @@ #pragma once -#include "software/ai/passing/pass.h" +#include "software/ai/passing/base_pass.h" struct PassWithRating { - Pass pass; + BasePass pass; double rating; };