diff --git a/.github/workflows/tests-action.yml b/.github/workflows/tests-action.yml index d4a39d014..cf5dcd7f9 100644 --- a/.github/workflows/tests-action.yml +++ b/.github/workflows/tests-action.yml @@ -48,6 +48,18 @@ jobs: cd protobuf sudo make install sudo ldconfig + + - name: Build & Install Ruckig + if: steps.cache-build-restore.outputs.cache-hit != 'true' + run: | + git clone https://github.com/pantor/ruckig.git ruckig + cd ruckig + git checkout dae82835ec043a54ed6bc775f0776e1a7ff99124 + mkdir build + cd build + cmake .. -DCMAKE_BUILD_TYPE=Release + make -j $(nproc) + sudo make install - name: Build tests if: steps.cache-build-restore.outputs.cache-hit != 'true' diff --git a/CMakeLists.txt b/CMakeLists.txt index 762c70992..7d8e1eb71 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,6 +95,7 @@ if(UNIX AND NOT APPLE) endif() find_package(Qt5Network REQUIRED) +find_package(ruckig REQUIRED) # internal projects add_subdirectory(roboteam_networking) diff --git a/docker/Dockerfile b/docker/Dockerfile index 690d2a44a..6939c7cf7 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -36,6 +36,15 @@ RUN apk add --no-cache bash build-base cmake make musl-dev libtool \ qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev RUN npm install -g yarn +WORKDIR /tmp +RUN git clone https://github.com/pantor/ruckig.git /tmp/ruckig \ + && cd /tmp/ruckig \ + && git checkout dae82835ec043a54ed6bc775f0776e1a7ff99124 \ + && mkdir build && cd build \ + && cmake -DCMAKE_BUILD_TYPE=Release .. \ + && make -j$(nproc) \ + && make install + WORKDIR /root/protobuf RUN wget https://github.com/protocolbuffers/protobuf/releases/download/v3.20.3/protobuf-cpp-3.20.3.zip -O protobuf.zip && unzip protobuf.zip && rm protobuf.zip WORKDIR /root/protobuf/protobuf-3.20.3 @@ -70,7 +79,17 @@ RUN apk add --no-cache \ sudo nodejs npm RUN npm install -g yarn +WORKDIR /tmp +RUN git clone https://github.com/pantor/ruckig.git /tmp/ruckig \ + && cd /tmp/ruckig \ + && git checkout dae82835ec043a54ed6bc775f0776e1a7ff99124 \ + && mkdir build && cd build \ + && cmake -DCMAKE_BUILD_TYPE=Release .. \ + && make -j$(nproc) \ + && make install + COPY --from=development --chown=root:root /usr/local/lib/libprotobuf.so.31 /usr/local/lib/libprotobuf.so.31 +COPY --from=development --chown=root:root /usr/local/lib/libruckig.so /usr/local/lib/libruckig.so COPY --from=development --chown=root:root /usr/local/bin/protoc /usr/local/bin/protoc RUN adduser -D $USER && \ diff --git a/roboteam_ai/CMakeLists.txt b/roboteam_ai/CMakeLists.txt index 98337152b..8971c13e0 100644 --- a/roboteam_ai/CMakeLists.txt +++ b/roboteam_ai/CMakeLists.txt @@ -133,10 +133,12 @@ target_include_directories(roboteam_ai_control PRIVATE include/roboteam_ai ) target_link_libraries(roboteam_ai_control + PRIVATE ruckig PRIVATE roboteam_networking PRIVATE roboteam_utils PUBLIC Qt5::Network ) +target_compile_features(roboteam_ai_control PUBLIC cxx_std_17) target_compile_options(roboteam_ai_control PRIVATE "${COMPILER_FLAGS}") ###### INTERFACE ####### diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory2D.h b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory2D.h index 0055e8aa2..b718c1645 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory2D.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory2D.h @@ -1,11 +1,13 @@ #ifndef RTT_TRAJECTORY2D_H #define RTT_TRAJECTORY2D_H +#include #include #include "Trajectory1D.h" #include "roboteam_utils/Vector2.h" +using namespace ruckig; namespace rtt { /** @@ -19,15 +21,11 @@ class Trajectory2D { */ Trajectory2D() = default; - /** - * @brief Constructor of the Trajectory2D class - * @param initialPos Initial position of the robot - * @param initialVel Initial velocity of the robot - * @param finalPos Target position to got to - * @param maxVel Maximum allowed velocity - * @param maxAcc Maximum allowed acceleration or deceleration - */ - Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc); + Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc, double maxJerk, int robotId); + Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, const Vector2 &finalVel, double maxVel, double maxAcc, double maxJerk, int robotId); + Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &initialAcc, const Vector2 &finalPos, double maxVel, double maxAcc, double maxJerk); + Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &initialAcc, const Vector2 &finalPos, const Vector2 &finalVel, double maxVel, double maxAcc, + double maxJerk); /** * @brief Stores the trajectory @@ -76,10 +74,16 @@ class Trajectory2D { * @return Total time of the trajectory to end point */ [[nodiscard]] double getTotalTime() const; + static std::unordered_map lastAcceleration; /**< Map of last acceleration for each robot */ + + Vector2 getLastAcceleration(int robotId); private: - Trajectory1D x; /**< 1D x component of the 2D Trajectory */ - Trajectory1D y; /**< 1D y component of the 2D Trajectory */ + Trajectory<1> x; /**< 1D x component of the 2D Trajectory */ + std::optional> xAdded; + Trajectory<1> y; /**< 1D y component of the 2D Trajectory */ + std::optional> yAdded; + double addedFromTime = std::numeric_limits::infinity(); }; } // namespace rtt diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index 50b7253a1..c828064d9 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -40,11 +40,13 @@ class PositionControl { * @param currentPosition the current position of the aforementioned robot * @param currentVelocity its velocity * @param targetPosition the desired position that the robot has to reach + * @param targetVelocity the desired velocity that the robot has to reach * @param maxRobotVelocity the maximum velocity that the robot is allowed to have + * @param maxJerk the maximum jerk that the robot is allowed to have * @return A RobotCommand and optional with the location of the first collision on the path */ - Vector2 computeAndTrackTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity, - Vector2 targetPosition, double maxRobotVelocity, stp::AvoidObjects avoidObjects); + std::pair computeAndTrackTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity, + Vector2 targetPosition, Vector2 targetVelocity, double maxRobotVelocity, double maxJerk, stp::AvoidObjects avoidObjects); /** * @brief Handles the collision with the ball at the current position. This function will calculate a new target, moving away from the ball as quickly as possible. @@ -90,7 +92,7 @@ class PositionControl { * @return An optional with a new path */ Trajectory2D findNewTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 ¤tPosition, Vector2 ¤tVelocity, - Vector2 &targetPosition, double maxRobotVelocity, double timeStep, stp::AvoidObjects AvoidObjects); + Vector2 &targetPosition, Vector2 &targetVelocity, double maxRobotVelocity, double maxJerk, double timeStep, stp::AvoidObjects AvoidObjects); /** * @brief Creates normalized random points, which will be used to create intermediate points diff --git a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h index bada16d34..a66a9203b 100644 --- a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h +++ b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h @@ -32,6 +32,12 @@ struct StpInfo { void setPositionToMoveTo(const std::optional& position) { this->positionToMoveTo = position; } void setPositionToMoveTo(const std::optional& scoredPosition) { setPositionToMoveTo(scoredPosition->position); } + const Vector2& getTargetVelocity() const { return targetVelocity; } + void setTargetVelocity(const Vector2& targetVelocity) { this->targetVelocity = targetVelocity; } + + double getMaxJerk() const { return maxJerk; } + void setMaxJerk(double maxJerk) { this->maxJerk = maxJerk; } + const std::optional& getPositionToShootAt() const { return positionToShootAt; } void setPositionToShootAt(const std::optional& position) { this->positionToShootAt = position; } @@ -50,6 +56,9 @@ struct StpInfo { ShotPower getShotPower() const { return shotPower; } void setShotPower(ShotPower shotPower) { this->shotPower = shotPower; } + bool getShootOnFirstTouch() const { return shootOnFirstTouch; } + void setShootOnFirstTouch(bool shootOnFirstTouch) { this->shootOnFirstTouch = shootOnFirstTouch; } + rtt::KickType getKickOrChip() const { return kickOrChip; } void setKickOrChip(rtt::KickType kickOrChip) { this->kickOrChip = kickOrChip; } @@ -133,6 +142,16 @@ struct StpInfo { */ std::optional positionToMoveTo; + /** + * Target velocity to move with + */ + Vector2 targetVelocity; + + /** + * Jerk to move with + */ + double maxJerk = ai::constants::MAX_JERK_DEFAULT; + /** * Position to kick or chip at */ @@ -153,6 +172,11 @@ struct StpInfo { */ ShotPower shotPower{}; + /** + * Shoot as soon as we see the ball, also when we are not in a kick skill + */ + bool shootOnFirstTouch = false; + /** * Reference yaw of the robot */ diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h index 22568a0cf..7ee15df03 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h @@ -21,6 +21,7 @@ namespace rtt::ai::stp { struct InterceptionInfo { Vector2 interceptLocation; + Vector2 interceptVelocity; int interceptId = -1; double timeToIntercept = std::numeric_limits::max(); bool isInterceptable = false; diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h index 4c588e6b1..60d437e56 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h @@ -55,34 +55,40 @@ class PassComputations { * @param point the point to check for validity * @param possibleReceiverLocations the locations of all robots that could receive the ball * @param possibleReceiverVelocities the velocities of all robots that could receive the ball + * @param possiblePassLocations the locations of all robots that could pass the ball * @param passLocation the location where the ball will be passed from * @param passerLocation the location of the robot that will pass the ball * @param passerVelocity the velocity of the robot that will pass the ball + * @param passerId the id of the robot that will pass the ball * @param field the current field * @param world the current world * @return bool indicating whether this point is (likely) possible to pass to */ + static bool pointIsValidReceiverLocation(Vector2 point, const std::vector& possibleReceiverLocations, const std::vector& possibleReceiverVelocities, - Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, const Field& field, const world::World* world); + const std::vector& possibleReceiverIds, Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, int passerId, + const Field& field, const world::World* world); /** * @brief Approximate the time it takes a robot to reach a point * @param robotPosition current position of robot * @param robotVelocity current velocity of robot + * @param robotId id of robot * @param targetPosition position to calculate travel time to * @return approximated time to reach target from position */ - static double calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, Vector2 targetPosition); + static double calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, int robotId, Vector2 targetPosition); /** * Approximate the time it takes the ball to reach a point * @param passLocation the location where the ball will be passed from * @param passerLocation current location of the passer + * @param passerId id of the passer * @param robotVelocity current velocity of robot * @param targetPosition position to calculate travel time to * @return approximated time for ball to reach target position */ - static double calculateBallTravelTime(Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, Vector2 targetPosition); + static double calculateBallTravelTime(Vector2 passLocation, Vector2 passerLocation, int passerId, Vector2 passerVelocity, Vector2 targetPosition); }; } // namespace rtt::ai::stp::computations #endif // RTT_PASSCOMPUTATIONS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/roles/active/BallPlacer.h b/roboteam_ai/include/roboteam_ai/stp/roles/active/BallPlacer.h index 98e02afec..e1c708aa5 100644 --- a/roboteam_ai/include/roboteam_ai/stp/roles/active/BallPlacer.h +++ b/roboteam_ai/include/roboteam_ai/stp/roles/active/BallPlacer.h @@ -14,6 +14,7 @@ class BallPlacer : public Role { * @param name The name of the role */ BallPlacer(std::string name); + [[nodiscard]] Status update(StpInfo const& info) noexcept override; }; } // namespace rtt::ai::stp::role diff --git a/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h b/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h index 1a69e8c83..7ffa6309a 100644 --- a/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h +++ b/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h @@ -72,7 +72,7 @@ class KeeperBlockBall : public Tactic { * @param info the StpInfo struct * @return Target position for the keeper and a bool indicating if the keeper should avoid goal posts */ - static std::pair calculateTargetPosition(const StpInfo info) noexcept; + static std::tuple calculateTargetPosition(const StpInfo info) noexcept; /** * @brief Calculates the target position for the keeper when the ball is shot @@ -80,7 +80,7 @@ class KeeperBlockBall : public Tactic { * @param keepersLineSegment the lineSegment of the goal * @param ballTrajectory the trajectory of the ball */ - static Vector2 calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept; + static std::pair calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept; /** * @brief Calculates the target position for the keeper when the ball is not shot diff --git a/roboteam_ai/include/roboteam_ai/utilities/Constants.h b/roboteam_ai/include/roboteam_ai/utilities/Constants.h index dc2708641..528c23a11 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/Constants.h +++ b/roboteam_ai/include/roboteam_ai/utilities/Constants.h @@ -46,14 +46,18 @@ constexpr double MIN_YAW = -M_PI; /**< Minimum yaw the robot can hav constexpr double MAX_YAW = M_PI; /**< Maximum yaw the robot can have */ constexpr double MAX_ACC = 3.5; /**< Maximum acceleration of the robot */ constexpr double MAX_VEL = 4.0; /**< Maximum allowed velocity of the robot */ +constexpr double MAX_JERK_OVERSHOOT = 100; /**< Jerk limit for overshoot */ +// TODO ROBOCUP 2024: FIX THIS MAGIC +constexpr double MAX_JERK_DEFAULT = 6; /**< Default jerk limit */ /// GoToPos Constants // Distance margin for 'goToPos'. If the robot is within this margin, goToPos is successful -constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot to be considered to have reached a position */ +constexpr double GO_TO_POS_ERROR_MARGIN = 0.02; /**< Distance error for a robot to be considered to have reached a position */ // Angle margin for 'goToPos'. If the robot is within this margin, goToPos is successful constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.01; /**< Angle error for a robot to be considered to have reached a position */ // Maximum inaccuracy during ballplacement -constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS - 0.02; /**< Distance error for the ball to be considered to have reached the ball placement position*/ +constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS - 0.01; /**< Distance error for the ball to be considered to have reached the ball placement position*/ +constexpr double BALL_PLACER_MARGIN = BALL_PLACEMENT_MARGIN - 0.05; /**< Distance before the ball placer moves away from hte ball*/ constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9; /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */ /// Invariant constants @@ -64,21 +68,24 @@ constexpr double FUZZY_DEFAULT_CUTOFF = 127; /**< Value at which the fuzzy logic /// Distance constants constexpr double ROBOT_CLOSE_TO_POINT = 0.2; /**< Distance from the robot to a position at which the robot is considered close to that position */ constexpr double DISTANCE_TO_PASS_TRAJECTORY = 0.5; /**< Distance from the robot to the pass trajectory at which the robot is considered too close to the pass trajectory */ -constexpr double OUT_OF_FIELD_MARGIN = 0.17; /**< Distance that the center of the robot is allowed to go out of the field during play (not for end location, only for paths) */ +constexpr double OUT_OF_FIELD_MARGIN = 0.2; /**< Distance that the center of the robot is allowed to go out of the field during play (not for end location, only for paths) */ constexpr double FREE_KICK_TAKEN_DISTANCE = 0.07; /**< Distance that the ball must travel before we can say a kickoff or free kick has been taken */ constexpr double PENALTY_DISTANCE_BEHIND_BALL = 1.5; /**< The minimum distance the robots have to be behind the ball during a penalty, default is 1 meter, but do 1.5 to be sure */ ; /// GameState constants -constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.1; /**< Minimum distance all robots should keep when avoiding the ball */ +constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + BALL_RADIUS + 0.05; /**< Minimum distance all robots should keep when avoiding the ball */ constexpr double AVOID_BALL_DISTANCE_BEFORE_FREE_KICK = - 0.05 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.01; /**< Minimum distance all robots should keep when avoiding the ball before a free kick */ + 0.05 + ROBOT_RADIUS + BALL_RADIUS + 0.01; /**< Minimum distance all robots should keep when avoiding the ball before a free kick */ /// Friction constants constexpr static float SIMULATION_FRICTION = 0.71; /**< The expected movement friction of the ball during simulation */ constexpr static float REAL_FRICTION = 0.44; /**< The expected movement friction of the ball on the field */ static inline double HAS_BALL_DISTANCE() { return (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? 0.11 : 0.12; } +static inline double SEND_TIME_IN_FUTURE() { + return (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? 0.04 : 0.145; +} // Simulator doesn't have mass feedforward, so we need to send a bigger difference static std::map ROBOTS_WITH_WORKING_DRIBBLER() { static std::map workingDribblerRobots; diff --git a/roboteam_ai/include/roboteam_ai/utilities/Dealer.h b/roboteam_ai/include/roboteam_ai/utilities/Dealer.h index 0cb3d7896..a01582b30 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/Dealer.h +++ b/roboteam_ai/include/roboteam_ai/utilities/Dealer.h @@ -145,9 +145,10 @@ class Dealer { * @param robot the robot for which the cost will be determined * @param target_position the position the robot will be travelling to * @param MaxRobotVelocity the maximum velocity a robot can have + * @param MaxRobotJerk the maximum jerk a robot can have * @return Cost of travelling that distance */ - static double costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity); + static double costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity, const double MaxRobotJerk); /** * @brief Calculates the cost of a property of a robot diff --git a/roboteam_ai/src/control/ControlModule.cpp b/roboteam_ai/src/control/ControlModule.cpp index 77f0ed143..66e9ff8f4 100644 --- a/roboteam_ai/src/control/ControlModule.cpp +++ b/roboteam_ai/src/control/ControlModule.cpp @@ -12,6 +12,8 @@ namespace rtt::ai::control { void ControlModule::rotateRobotCommand(rtt::RobotCommand& command) { command.velocity.x = -command.velocity.x; command.velocity.y = -command.velocity.y; + command.acceleration.x = -command.acceleration.x; + command.acceleration.y = -command.acceleration.y; command.yaw += M_PI; } diff --git a/roboteam_ai/src/control/ControlUtils.cpp b/roboteam_ai/src/control/ControlUtils.cpp index cd12915a2..05d9c9598 100644 --- a/roboteam_ai/src/control/ControlUtils.cpp +++ b/roboteam_ai/src/control/ControlUtils.cpp @@ -23,7 +23,7 @@ double ControlUtils::determineKickForce(const double distance, stp::ShotPower sh double kickForce; if (shotPower == stp::ShotPower::PASS) { - kickForce = 1.8; + kickForce = 0.8; } else if (shotPower == stp::ShotPower::TARGET) { kickForce = 0.5; } else { diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/Trajectory2D.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/Trajectory2D.cpp index 2092ec2b4..6babac352 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/Trajectory2D.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/Trajectory2D.cpp @@ -1,26 +1,103 @@ #include "control/positionControl/BBTrajectories/Trajectory2D.h" #include +#include #include "control/positionControl/BBTrajectories/BBTrajectory2D.h" #include "roboteam_utils/Print.h" +using namespace ruckig; namespace rtt { +std::unordered_map Trajectory2D::lastAcceleration; -Trajectory2D::Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc) { - rtt::ai::control::BBTrajectory2D BBTNoCollision = rtt::ai::control::BBTrajectory2D(initialPos, initialVel, finalPos, maxVel, maxAcc); - std::pair, std::vector> parts = BBTNoCollision.getParts(); - x.parts = parts.first; - x.finalPos = finalPos.x; - y.parts = parts.second; - y.finalPos = finalPos.y; +Vector2 Trajectory2D::getLastAcceleration(int robotId) { + if (Trajectory2D::lastAcceleration.contains(robotId)) { + return Trajectory2D::lastAcceleration[robotId]; + } else { + return Vector2(0, 0); + } +} +Trajectory2D::Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc, double maxJerk, int robotId) + : Trajectory2D(initialPos, initialVel, getLastAcceleration(robotId), finalPos, Vector2(0, 0), maxVel, maxAcc, maxJerk) {} + +Trajectory2D::Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, const Vector2 &finalVel, double maxVel, double maxAcc, double maxJerk, + int robotId) + : Trajectory2D(initialPos, initialVel, getLastAcceleration(robotId), finalPos, finalVel, maxVel, maxAcc, maxJerk) {} + +Trajectory2D::Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &initialAcc, const Vector2 &finalPos, double maxVel, double maxAcc, double maxJerk) + : Trajectory2D(initialPos, initialVel, initialAcc, finalPos, Vector2(0, 0), maxVel, maxAcc, maxJerk) {} + +Trajectory2D::Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &initialAcc, const Vector2 &finalPos, const Vector2 &finalVel, double maxVel, + double maxAcc, double maxJerk) { + // The idea is to do a binary search over alpha to find a trajectory in x and y direction (which is minimal time) + double inc = M_PI_4 * 0.5; + double alpha = M_PI_4; + constexpr double iterationLimit = 1e-7; + constexpr double timeDiffLimit = 1e-3; + while (inc > iterationLimit) { + InputParameter<1> inputX; + inputX.current_position[0] = initialPos.x; + if (initialVel.x > 0) { + inputX.current_velocity[0] = std::min(initialVel.x, std::max(maxVel * cos(alpha), std::abs(finalVel.x))); + } else { + inputX.current_velocity[0] = std::max(initialVel.x, -std::max(maxVel * cos(alpha), std::abs(finalVel.x))); + } + inputX.current_acceleration[0] = initialAcc.x; + inputX.target_position[0] = finalPos.x; + inputX.target_velocity[0] = finalVel.x; + inputX.target_acceleration[0] = 0; + inputX.max_velocity[0] = std::max(maxVel * cos(alpha), std::abs(finalVel.x)); + inputX.max_acceleration[0] = maxAcc * cos(alpha); + inputX.max_jerk[0] = maxJerk * cos(alpha); + Ruckig<1> ruckigX; + ruckigX.calculate(inputX, x); + + InputParameter<1> inputY; + inputY.current_position[0] = initialPos.y; + if (initialVel.y > 0) { + inputY.current_velocity[0] = std::min(initialVel.y, std::max(maxVel * sin(alpha), std::abs(finalVel.y))); + } else { + inputY.current_velocity[0] = std::max(initialVel.y, -std::max(maxVel * sin(alpha), std::abs(finalVel.y))); + } + inputY.current_acceleration[0] = initialAcc.y; + inputY.target_position[0] = finalPos.y; + inputY.target_velocity[0] = finalVel.y; + inputY.target_acceleration[0] = 0; + inputY.max_velocity[0] = std::max(maxVel * sin(alpha), std::abs(finalVel.y)); + inputY.max_acceleration[0] = maxAcc * sin(alpha); + inputY.max_jerk[0] = maxJerk * sin(alpha); + Ruckig<1> ruckigY; + ruckigY.calculate(inputY, y); + + double diff = abs(x.get_duration() - y.get_duration()); + // If the trajectories match enough we stop earlier + if (diff < timeDiffLimit) { + return; + } + if (x.get_duration() > y.get_duration()) { + alpha -= inc; + } else { + alpha += inc; + } + inc *= 0.5; + } + + // rtt::ai::control::BBTrajectory2D BBTNoCollision = rtt::ai::control::BBTrajectory2D(initialPos, initialVel, finalPos, maxVel, maxAcc); + // std::pair, std::vector> parts = BBTNoCollision.getParts(); + // x.parts = parts.first; + // x.finalPos = finalPos.x; + // y.parts = parts.second; + // y.finalPos = finalPos.y; } void Trajectory2D::addTrajectory(const Trajectory2D &extraTrajectory, double addFromTime) { - x.addTrajectory(extraTrajectory.x.parts, addFromTime); - x.finalPos = extraTrajectory.x.finalPos; - y.addTrajectory(extraTrajectory.y.parts, addFromTime); - y.finalPos = extraTrajectory.y.finalPos; + xAdded = extraTrajectory.x; + yAdded = extraTrajectory.y; + addedFromTime = addFromTime; + // x.addTrajectory(extraTrajectory.x.parts, addFromTime); + // x.finalPos = extraTrajectory.x.finalPos; + // y.addTrajectory(extraTrajectory.y.parts, addFromTime); + // y.finalPos = extraTrajectory.y.finalPos; } std::vector Trajectory2D::getPathApproach(double timeStep) const { @@ -59,12 +136,60 @@ std::vector Trajectory2D::getVelocityVector(double timeStep) const { return velocities; } -Vector2 Trajectory2D::getPosition(double t) const { return Vector2(x.getPosition(t), y.getPosition(t)); } +Vector2 Trajectory2D::getPosition(double t) const { + double newPosX, newVelX, newAccX, newPosY, newVelY, newAccY; + + if (xAdded && t > addedFromTime) { + xAdded->at_time(t, newPosX, newVelX, newAccX); + } else { + x.at_time(t, newPosX, newVelX, newAccX); + } + + if (yAdded && t > addedFromTime) { + yAdded->at_time(t, newPosY, newVelY, newAccY); + } else { + y.at_time(t, newPosY, newVelY, newAccY); + } + + return Vector2(newPosX, newPosY); +} + +Vector2 Trajectory2D::getVelocity(double t) const { + double newPosX, newVelX, newAccX, newPosY, newVelY, newAccY; -Vector2 Trajectory2D::getVelocity(double t) const { return Vector2(x.getVelocity(t), y.getVelocity(t)); } + if (xAdded && t > addedFromTime) { + xAdded->at_time(t, newPosX, newVelX, newAccX); + } else { + x.at_time(t, newPosX, newVelX, newAccX); + } + + if (yAdded && t > addedFromTime) { + yAdded->at_time(t, newPosY, newVelY, newAccY); + } else { + y.at_time(t, newPosY, newVelY, newAccY); + } + + return Vector2(newVelX, newVelY); +} + +Vector2 Trajectory2D::getAcceleration(double t) const { + double newPosX, newVelX, newAccX, newPosY, newVelY, newAccY; -Vector2 Trajectory2D::getAcceleration(double t) const { return Vector2(x.getAcceleration(t), y.getAcceleration(t)); } + if (xAdded && t > addedFromTime) { + xAdded->at_time(t, newPosX, newVelX, newAccX); + } else { + x.at_time(t, newPosX, newVelX, newAccX); + } + + if (yAdded && t > addedFromTime) { + yAdded->at_time(t, newPosY, newVelY, newAccY); + } else { + y.at_time(t, newPosY, newVelY, newAccY); + } + + return Vector2(newAccX, newAccY); +} -double Trajectory2D::getTotalTime() const { return std::max(x.getTotalTime(), y.getTotalTime()); } +double Trajectory2D::getTotalTime() const { return std::max(x.get_duration(), y.get_duration()); } } // namespace rtt \ No newline at end of file diff --git a/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp b/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp index 5623c202a..84236b9d8 100644 --- a/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp +++ b/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp @@ -32,7 +32,6 @@ double CollisionCalculations::getFirstCollisionTimeMotionlessObject(const Trajec pathLine.closestDistanceToLineSegment(rightGoalTopPost) < constants::ROBOT_RADIUS || pathLine.closestDistanceToLineSegment(rightGoalBottomPost) < constants::ROBOT_RADIUS || pathLine.closestDistanceToLineSegment(rightGoalBackPost) < constants::ROBOT_RADIUS) { - return checkPoint * 0.1; } } if (avoidObjects.shouldAvoidOurDefenseArea) { @@ -74,7 +73,7 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory double velocityOurRobot = Trajectory.getVelocity(checkPoint * 0.1).length(); auto positionOurRobot = Trajectory.getPosition(checkPoint * 0.1); double additionalMargin = std::pow(std::min(maxVel, velocityOurRobot) / maxVel, 2) * 0.2; - if (velocityOurRobot > 0.7 && avoidObjects.shouldAvoidOurRobots) { + if (velocityOurRobot > 0.4 && avoidObjects.shouldAvoidOurRobots) { for (const auto &ourOtherRobot : ourRobots) { const int &ourOtherRobotId = ourOtherRobot->getId(); if (ourOtherRobotId == robotId) { @@ -94,7 +93,7 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory } else { pathLineOtherRobot = LineSegment(computedPathsIt->second.back(), computedPathsIt->second.back()); } - if (pathLineOtherRobot.closestDistanceToLineSegment(pathLine) < 2 * constants::ROBOT_RADIUS) { + if (pathLineOtherRobot.closestDistanceToLineSegment(pathLine) < 2 * constants::ROBOT_RADIUS + additionalMargin) { return checkPoint * 0.1; } } diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 225c8d632..c29f001de 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -10,29 +10,42 @@ namespace rtt::ai::control { std::vector PositionControl::getComputedPath(int ID) { return computedPaths[ID]; } -Vector2 PositionControl::computeAndTrackTrajectory(const world::World *world, const Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity, - Vector2 targetPosition, double maxRobotVelocity, stp::AvoidObjects avoidObjects) { +std::pair PositionControl::computeAndTrackTrajectory(const world::World *world, const Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity, + Vector2 targetPosition, Vector2 targetVelocity, double maxRobotVelocity, double maxJerk, + stp::AvoidObjects avoidObjects) { double timeStep = 0.1; auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = FieldComputations::getDefenseAreaMargin(); if (avoidObjects.shouldAvoidBall && (currentPosition - world->getWorld()->getBall()->get()->position).length() < ai::constants::AVOID_BALL_DISTANCE) { targetPosition = handleBallCollision(world, field, currentPosition, avoidObjects); - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC, maxJerk, robotId); } else if ((GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM || GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) && LineSegment(world->getWorld()->getBall()->get()->position, GameStateManager::getRefereeDesignatedPosition()).distanceToLine(currentPosition) < ai::constants::AVOID_BALL_DISTANCE) { targetPosition = handleBallPlacementCollision(world, field, currentPosition, targetPosition, avoidObjects); - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC, maxJerk, robotId); } else if ((avoidObjects.shouldAvoidOurDefenseArea && FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1).contains(currentPosition)) || (avoidObjects.shouldAvoidTheirDefenseArea && theirDefenseAreaMargin > constants::ROBOT_RADIUS + constants::GO_TO_POS_ERROR_MARGIN && FieldComputations::getDefenseArea(field, false, theirDefenseAreaMargin, 1).contains(currentPosition))) { targetPosition = handleDefenseAreaCollision(field, currentPosition); - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC, maxJerk, robotId); + } else if (avoidObjects.shouldAvoidOutOfField && !field.playArea.contains(currentPosition, constants::OUT_OF_FIELD_MARGIN)) { + targetPosition = FieldComputations::projectPointInField(field, currentPosition, constants::OUT_OF_FIELD_MARGIN + 0.1); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC, maxJerk, robotId); + } else if (avoidObjects.shouldAvoidGoalPosts && ((field.leftGoalArea.topLine().distanceToLine(currentPosition) < constants::ROBOT_RADIUS - 0.02) || + (field.leftGoalArea.bottomLine().distanceToLine(currentPosition) < constants::ROBOT_RADIUS - 0.02) || + (field.leftGoalArea.leftLine().distanceToLine(currentPosition) < constants::ROBOT_RADIUS - 0.02) || + (field.rightGoalArea.topLine().distanceToLine(currentPosition) < constants::ROBOT_RADIUS - 0.02) || + (field.rightGoalArea.bottomLine().distanceToLine(currentPosition) < constants::ROBOT_RADIUS - 0.02) || + (field.rightGoalArea.rightLine().distanceToLine(currentPosition) < constants::ROBOT_RADIUS - 0.02))) { + targetPosition = Vector2(0, 0); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC, maxJerk, robotId); } else { - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, targetVelocity, maxRobotVelocity, ai::constants::MAX_ACC, maxJerk, robotId); auto hasCollsion = CollisionCalculations::isColliding(computedTrajectories[robotId], avoidObjects, field, robotId, world, computedPaths); if (hasCollsion) { - computedTrajectories[robotId] = findNewTrajectory(world, field, robotId, currentPosition, currentVelocity, targetPosition, maxRobotVelocity, timeStep, avoidObjects); + computedTrajectories[robotId] = + findNewTrajectory(world, field, robotId, currentPosition, currentVelocity, targetPosition, targetVelocity, maxRobotVelocity, maxJerk, timeStep, avoidObjects); } } computedPaths[robotId] = computedTrajectories[robotId].getPathApproach(0.05); @@ -47,16 +60,18 @@ Vector2 PositionControl::computeAndTrackTrajectory(const world::World *world, co .thickness = 1, }, computedPaths[robotId]); - // Since our acceleration is 3.5 m/s^2, this means we will move if we need to move more than - // 2*(1/2*3.5*0.145^2) (2 times the distance we move in 0.145/2s under constant (de)acceleration (1/2 * a * t^2)) - // This is 0.07m, so we will move if we need to move more than 0.07m - return computedTrajectories[robotId].getVelocity(0.145); + + auto acc = computedTrajectories[robotId].getAcceleration(constants::SEND_TIME_IN_FUTURE()); + auto vel = computedTrajectories[robotId].getVelocity(constants::SEND_TIME_IN_FUTURE()); + // TODO ROBOCUP 2024: Tweak this magic number + Trajectory2D::lastAcceleration[robotId] = computedTrajectories[robotId].getAcceleration(0.04); + return std::make_pair(vel, acc); } Vector2 PositionControl::handleBallCollision(const world::World *world, const Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects) { auto ballPos = world->getWorld()->getBall()->get()->position; auto direction = currentPosition - ballPos; - Vector2 targetPosition = currentPosition + direction.stretchToLength(constants::AVOID_BALL_DISTANCE * 2); + Vector2 targetPosition = currentPosition + direction.stretchToLength(constants::AVOID_BALL_DISTANCE); if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, constants::OUT_OF_FIELD_MARGIN)) { return targetPosition; } @@ -72,7 +87,7 @@ Vector2 PositionControl::handleBallCollision(const world::World *world, const Fi } } } - return currentPosition + direction.stretchToLength(constants::AVOID_BALL_DISTANCE * 2).rotate(M_PI / 2); + return currentPosition + direction.stretchToLength(constants::AVOID_BALL_DISTANCE).rotate(M_PI / 2); } Vector2 PositionControl::handleBallPlacementCollision(const world::World *world, const Field &field, Vector2 currentPosition, Vector2 targetPosition, @@ -121,13 +136,14 @@ Vector2 PositionControl::handleDefenseAreaCollision(const Field &field, Vector2 } Trajectory2D PositionControl::findNewTrajectory(const world::World *world, const Field &field, int robotId, Vector2 ¤tPosition, Vector2 ¤tVelocity, - Vector2 &targetPosition, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { + Vector2 &targetPosition, Vector2 &targetVelocity, double maxRobotVelocity, double maxJerk, double timeStep, + stp::AvoidObjects avoidObjects) { std::vector normalizedPoints = generateNormalizedPoints(robotId); timeStep *= 2; Vector2 startToDest = targetPosition - currentPosition; for (const auto &normalizedPoint : normalizedPoints) { auto intermediatePoint = startToDest.rotate(normalizedPoint.angle()) * normalizedPoint.length() + currentPosition; - Trajectory2D trajectoryToIntermediatePoint(currentPosition, currentVelocity, intermediatePoint, maxRobotVelocity, ai::constants::MAX_ACC); + Trajectory2D trajectoryToIntermediatePoint(currentPosition, currentVelocity, intermediatePoint, maxRobotVelocity, maxJerk, ai::constants::MAX_ACC, robotId); auto timeTillFirstCollision = CollisionCalculations::getFirstCollisionTime(trajectoryToIntermediatePoint, avoidObjects, field, robotId, world, computedPaths); double maxLoopTime = timeTillFirstCollision != -1 ? timeTillFirstCollision - 0.1 : trajectoryToIntermediatePoint.getTotalTime(); int numSteps = static_cast(maxLoopTime / timeStep); @@ -135,7 +151,9 @@ Trajectory2D PositionControl::findNewTrajectory(const world::World *world, const double loopTime = i * timeStep; Vector2 newStartPosition = trajectoryToIntermediatePoint.getPosition(loopTime); Vector2 newStartVelocity = trajectoryToIntermediatePoint.getVelocity(loopTime); - Trajectory2D trajectoryFromIntermediateToTarget(newStartPosition, newStartVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); + Vector2 newStartAcceleration = trajectoryToIntermediatePoint.getAcceleration(loopTime); + Trajectory2D trajectoryFromIntermediateToTarget(newStartPosition, newStartVelocity, newStartAcceleration, targetPosition, targetVelocity, maxRobotVelocity, + ai::constants::MAX_ACC, maxJerk); auto hasCollision = CollisionCalculations::isColliding(trajectoryFromIntermediateToTarget, avoidObjects, field, robotId, world, computedPaths); if (!hasCollision) { trajectoryToIntermediatePoint.addTrajectory(trajectoryFromIntermediateToTarget, loopTime); @@ -145,7 +163,7 @@ Trajectory2D PositionControl::findNewTrajectory(const world::World *world, const } } lastUsedNormalizedPoints.erase(robotId); - return Trajectory2D(currentPosition, currentVelocity, currentPosition, maxRobotVelocity, ai::constants::MAX_ACC); + return Trajectory2D(currentPosition, currentVelocity, currentPosition, maxRobotVelocity, ai::constants::MAX_ACC, maxJerk, robotId); ; } diff --git a/roboteam_ai/src/stp/computations/InterceptionComputations.cpp b/roboteam_ai/src/stp/computations/InterceptionComputations.cpp index d3beacfed..4120e7c74 100644 --- a/roboteam_ai/src/stp/computations/InterceptionComputations.cpp +++ b/roboteam_ai/src/stp/computations/InterceptionComputations.cpp @@ -36,7 +36,8 @@ KeeperInterceptionInfo InterceptionComputations::calculateKeeperInterceptionInfo return keeperInterceptionInfo; } // If we can reach the ball in time, we will intercept - if (Trajectory2D(keeper->getPos(), keeper->getVel(), futureBallPosition, maxRobotVelocity, ai::constants::MAX_ACC).getTotalTime() < loopTime) { + if (Trajectory2D(keeper->getPos(), keeper->getVel(), futureBallPosition, maxRobotVelocity, ai::constants::MAX_ACC, ai::constants::MAX_JERK_DEFAULT, keeper->getId()) + .getTotalTime() < loopTime) { keeperInterceptionInfo.interceptLocation = futureBallPosition; keeperInterceptionInfo.canIntercept = true; return keeperInterceptionInfo; @@ -59,7 +60,7 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: auto interceptRobot = calculateTheirBallInterception(world, ballTrajectory); // Helper function to calculate the intercept info for a given target position - auto calculateIntercept = [&](const Vector2 &targetPosition) { + auto calculateIntercept = [&](const Vector2 &targetPosition, const Vector2 &targetVelocity) { // Check if they are able to intercept the ball at the part of the trajectory we are checking if (interceptRobot && (*interceptRobot - pastBallPosition).length() < (futureBallPosition - pastBallPosition).length()) { auto minDistance = (*interceptRobot - pastBallPosition).length(); @@ -72,21 +73,30 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: } double minTimeToTarget = std::numeric_limits::max(); for (const auto &robot : ourRobots) { - auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), interceptionInfo.interceptLocation, maxRobotVelocity, ai::constants::MAX_ACC); + auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), interceptionInfo.interceptLocation, maxRobotVelocity, ai::constants::MAX_ACC, + ai::constants::MAX_JERK_DEFAULT, robot->getId()); if (trajectory.getTotalTime() < minTimeToTarget) { minTimeToTarget = trajectory.getTotalTime(); interceptionInfo.interceptId = robot->getId(); interceptionInfo.timeToIntercept = minTimeToTarget; } } + interceptionInfo.interceptVelocity = Vector2(0, 0); shouldReturn = true; return; } for (const auto &robot : ourRobots) { + auto ballToRobot = (robot->getPos() - ballPosition); + auto ballToFutPos = (futureBallPosition - ballPosition); + // FutPos and Robot are on opposite sides of the ball, don't consider this robot + if (ballToRobot.dot(ballToFutPos) < 0) { + continue; + } // If the robot is already close to the line, project it's position onto the line to prevent always moving to the 0.1 second mark if (LineSegment(pastBallPosition, futureBallPosition).distanceToLine(robot->getPos()) < 1.5 * constants::ROBOT_RADIUS) { interceptionInfo.interceptLocation = LineSegment(pastBallPosition, futureBallPosition).project(robot->getPos()); + interceptionInfo.interceptVelocity = Vector2(0, 0); interceptionInfo.interceptId = robot->getId(); interceptionInfo.timeToIntercept = 0; interceptionInfo.isInterceptable = true; @@ -96,10 +106,22 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: double minTimeToTarget = std::numeric_limits::max(); for (const auto &robot : ourRobots) { - auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); + Vector2 usedTargetVelocity; + auto robotToBall = (ballPosition - robot->getPos()); + auto angle = Angle(robotToBall).shortestAngleDiff(Angle(targetVelocity)); + // TODO ROBOCUP 2024: TWEAK THIS NOT MAGIC NUMBER + auto circle = Circle(ballPosition - targetVelocity.stretchToLength(0.5), 0.5); + if (std::abs(angle) < M_PI / 4 || circle.contains(robot->getPos())) { + usedTargetVelocity = targetVelocity; + } else { + usedTargetVelocity = Vector2(0, 0); + } + auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), targetPosition, usedTargetVelocity, maxRobotVelocity, ai::constants::MAX_ACC, + ai::constants::MAX_JERK_OVERSHOOT, robot->getId()); if (trajectory.getTotalTime() < minTimeToTarget) { minTimeToTarget = trajectory.getTotalTime(); interceptionInfo.interceptLocation = targetPosition; + interceptionInfo.interceptVelocity = usedTargetVelocity; interceptionInfo.interceptId = robot->getId(); interceptionInfo.timeToIntercept = minTimeToTarget; auto theirClosestToBall = world->getWorld()->getRobotClosestToBall(world::them); @@ -112,7 +134,8 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: // If the ball is not moving, we use the current ball position if (ballVelocity.length() <= constants::BALL_STILL_VEL) { - calculateIntercept(ballPosition); + calculateIntercept(ballPosition, Vector2(0, 0)); + interceptionInfo.interceptVelocity = Vector2(0, 0); return interceptionInfo; } @@ -146,14 +169,15 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: if (world->getField().value().playArea.contains(ballPosition, constants::BALL_RADIUS)) { futureBallPosition = FieldComputations::projectPointIntoFieldOnLine(world->getField().value(), futureBallPosition, ballPosition, futureBallPosition, constants::BALL_RADIUS); - calculateIntercept(futureBallPosition); + calculateIntercept(futureBallPosition, Vector2(0, 0)); } else { - calculateIntercept(ballPosition); + calculateIntercept(ballPosition, Vector2(0, 0)); } return interceptionInfo; } - - calculateIntercept(futureBallPosition); + // TODO ROBOCUP 2024: PLEASE FIX :{} + auto ballVelFuture = (futureBallPosition - pastBallPosition) / 0.1; + calculateIntercept(futureBallPosition, ballVelFuture); // If any robot can intercept the ball in time, return that info if (loopTime >= interceptionInfo.timeToIntercept) { interceptionInfo.isInterceptable = true; diff --git a/roboteam_ai/src/stp/computations/PassComputations.cpp b/roboteam_ai/src/stp/computations/PassComputations.cpp index 9fb239d36..c8bce72cc 100644 --- a/roboteam_ai/src/stp/computations/PassComputations.cpp +++ b/roboteam_ai/src/stp/computations/PassComputations.cpp @@ -41,10 +41,12 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w Vector2 passerLocation; Vector2 passerVelocity; + int passerId; // there should always be a valid passer, since we know there are >2 robots (or 2 robots where the keeper can pass/receive), but check just in case something goes wrong if (passerIt != us.end()) { passerLocation = passerIt->get()->getPos(); passerVelocity = passerIt->get()->getVel(); + passerId = passerIt->get()->getId(); us.erase(passerIt); } else { // If we could not find a passer, we return an empty passInfo @@ -54,19 +56,23 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w // This is a vector with the locations of all robots that could act as a receiver (ie all robots except the keeper and the passer) std::vector possibleReceiverLocations; std::vector possibleReceiverVelocities; + std::vector possibleReceiverIds; for (const auto& robot : us) { if (constants::ROBOT_HAS_KICKER(robot->getId())) { possibleReceiverLocations.push_back(robot->getPos()); possibleReceiverVelocities.push_back(robot->getVel()); + possibleReceiverIds.push_back(robot->getId()); } } // If there are no other robots that can kick, add every other robots if (possibleReceiverLocations.empty()) { possibleReceiverLocations.reserve(us.size()); possibleReceiverVelocities.reserve(us.size()); + possibleReceiverIds.reserve(us.size()); for (const auto& robot : us) { possibleReceiverLocations.push_back(robot->getPos()); possibleReceiverVelocities.push_back(robot->getVel()); + possibleReceiverIds.push_back(robot->getId()); } } @@ -77,7 +83,8 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w for (auto& point : pointVector) { numberOfPoints++; std::array pointToPassTo = {point}; - if (pointIsValidReceiverLocation(point, possibleReceiverLocations, possibleReceiverVelocities, passInfo.passLocation, passerLocation, passerVelocity, field, world)) { + if (pointIsValidReceiverLocation(point, possibleReceiverLocations, possibleReceiverVelocities, possibleReceiverIds, passInfo.passLocation, passerLocation, + passerVelocity, passerId, field, world)) { gen::ScoredPosition scoredPosition = PositionScoring::scorePosition(point, profile, field, world); rtt::ai::gui::Out::draw( { @@ -116,8 +123,10 @@ Grid PassComputations::getPassGrid(const Field& field) { } bool PassComputations::pointIsValidReceiverLocation(Vector2 point, const std::vector& possibleReceiverLocations, const std::vector& possibleReceiverVelocities, - Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, const Field& field, const world::World* world) { - constexpr double MINIMUM_PASS_DISTANCE = 2.0; // This can be dribbled instead of passed + const std::vector& possibleReceiverIds, Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, int passerId, + const Field& field, const world::World* world) { + // TODO ROBOCUP 2024: CHECK THIS + constexpr double MINIMUM_PASS_DISTANCE = 4.0; // This can be dribbled instead of passed if (point.dist(passLocation) < MINIMUM_PASS_DISTANCE) return false; constexpr double MINIMUM_LINE_OF_SIGHT = 10.0; // The minimum LoS to be a valid pass, otherwise, the pass will go into an enemy robot if (PositionScoring::scorePosition(point, gen::LineOfSight, field, world).score < MINIMUM_LINE_OF_SIGHT) return false; @@ -127,19 +136,22 @@ bool PassComputations::pointIsValidReceiverLocation(Vector2 point, const std::ve avoidObj.shouldAvoidOutOfField = true; if (!FieldComputations::pointIsValidPosition(field, point, avoidObj)) return false; // Pass is valid if the above conditions are met and there is a robot whose travel time is smaller than the balls travel time (i.e. the robot can actually receive the ball) - auto ballTravelTime = calculateBallTravelTime(passLocation, passerLocation, passerVelocity, point); + auto ballTravelTime = calculateBallTravelTime(passLocation, passerLocation, passerId, passerVelocity, point); for (std::vector::size_type i = 0; i < possibleReceiverLocations.size(); i++) { - if (calculateRobotTravelTime(possibleReceiverLocations[i], possibleReceiverVelocities[i], point) < ballTravelTime) return true; + if (calculateRobotTravelTime(possibleReceiverLocations[i], possibleReceiverVelocities[i], possibleReceiverIds[i], point) < ballTravelTime) return true; } return false; } -double PassComputations::calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, Vector2 targetPosition) { - return Trajectory2D(robotPosition, robotVelocity, targetPosition, control::ControlUtils::getMaxVelocity(false), ai::constants::MAX_ACC).getTotalTime() * 1.1; +double PassComputations::calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, int robotId, Vector2 targetPosition) { + return Trajectory2D(robotPosition, robotVelocity, targetPosition, control::ControlUtils::getMaxVelocity(false), ai::constants::MAX_ACC, ai::constants::MAX_JERK_DEFAULT, + robotId) + .getTotalTime() * + 1.1; } -double PassComputations::calculateBallTravelTime(Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, Vector2 targetPosition) { - auto travelTime = calculateRobotTravelTime(passerLocation, passerVelocity, passLocation - (passerLocation - passLocation).stretchToLength(constants::ROBOT_RADIUS)); +double PassComputations::calculateBallTravelTime(Vector2 passLocation, Vector2 passerLocation, int passerId, Vector2 passerVelocity, Vector2 targetPosition) { + auto travelTime = calculateRobotTravelTime(passerLocation, passerVelocity, passerId, passLocation - (passerLocation - passLocation).stretchToLength(constants::ROBOT_RADIUS)); auto rotateTime = (passLocation - passerLocation).toAngle().shortestAngleDiff(targetPosition - passLocation) / (M_PI); double ballSpeed = control::ControlUtils::determineKickForce(passLocation.dist(targetPosition), ShotPower::PASS); auto ballTime = passLocation.dist(targetPosition) / ballSpeed; diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 4bc788e3d..eea178727 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -339,7 +339,7 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_mapget()->hasBall() && enemyAngle.shortestAngleDiff(harasserAngle) < M_PI / 1.5) { auto enemyPos = enemyClosestToBall->get()->getPos(); - auto targetPos = enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(constants::ROBOT_RADIUS * 4 + constants::GO_TO_POS_ERROR_MARGIN); + auto targetPos = enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(constants::ROBOT_RADIUS * 4); stpInfos["harasser"].setPositionToMoveTo(targetPos); stpInfos["harasser"].setYaw((world->getWorld()->getBall()->get()->position - targetPos).angle()); } else { diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp index 2f19f72dc..a39a33a0a 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp @@ -86,12 +86,7 @@ void BallPlacementUsForceStart::calculateInfoForRoles() noexcept { stpInfos["ball_placer"].setShouldAvoidOutOfField(false); stpInfos["ball_placer"].setShouldAvoidBall(false); - if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < constants::BALL_PLACEMENT_MARGIN) { - for (auto& role : roles) - if (role->getName() == "ball_placer") role->forceLastTactic(); - } - - if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < 1.0) stpInfos["ball_placer"].setMaxRobotVelocity(0.75); + if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->hasBall()) stpInfos["ball_placer"].setMaxRobotVelocity(0.75); if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < constants::TURN_ON_DRIBBLER_DISTANCE) { stpInfos["ball_placer"].setDribblerOn(true); } diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp index 566da9ca1..d7fa9c999 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp @@ -86,11 +86,7 @@ void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { stpInfos["ball_placer"].setShouldAvoidOutOfField(false); stpInfos["ball_placer"].setShouldAvoidBall(false); - if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < constants::BALL_PLACEMENT_MARGIN) { - for (auto& role : roles) - if (role->getName() == "ball_placer") role->forceLastTactic(); - } - if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < 1.0) stpInfos["ball_placer"].setMaxRobotVelocity(0.75); + if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->hasBall()) stpInfos["ball_placer"].setMaxRobotVelocity(0.75); if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < constants::TURN_ON_DRIBBLER_DISTANCE) { stpInfos["ball_placer"].setDribblerOn(true); } diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp index 6ab0846e3..f028f4fe7 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp @@ -71,6 +71,7 @@ void FreeKickUsAtGoal::calculateInfoForRoles() noexcept { stpInfos["free_kick_taker"].setPositionToShootAt(goalTarget); stpInfos["free_kick_taker"].setKickOrChip(KickType::KICK); stpInfos["free_kick_taker"].setShotPower(ShotPower::MAX); + stpInfos["free_kick_taker"].setShootOnFirstTouch(true); PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, goalTarget); } diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp index 6223e5a57..495a9147d 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp @@ -74,10 +74,12 @@ void FreeKickUsPass::calculateInfoForRoles() noexcept { stpInfos["free_kick_taker"].setPositionToShootAt(passInfo.receiverLocation); stpInfos["free_kick_taker"].setShotPower(ShotPower::PASS); stpInfos["free_kick_taker"].setKickOrChip(KickType::KICK); + stpInfos["free_kick_taker"].setShootOnFirstTouch(true); if (RuntimeConfig::useReferee && GameStateManager::getCurrentGameState().timeLeft < 1.5) { stpInfos["free_kick_taker"].setPositionToShootAt(field.rightDefenseArea.rightLine().center()); stpInfos["free_kick_taker"].setShotPower(ShotPower::MAX); stpInfos["free_kick_taker"].setKickOrChip(KickType::CHIP); + stpInfos["free_kick_taker"].setShootOnFirstTouch(true); } } else { // Receiver goes to the receiverLocation projected on the trajectory of the ball diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp index e381fc80c..03512153f 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp @@ -65,8 +65,9 @@ Dealer::FlagMap KickOffUs::decideRoleFlags() const noexcept { void KickOffUs::calculateInfoForRoles() noexcept { Vector2 theirGoal = Vector2(6, 0); stpInfos["kick_off_taker"].setPositionToShootAt(theirGoal); - stpInfos["kick_off_taker"].setShotPower(ShotPower::PASS); + stpInfos["kick_off_taker"].setShotPower(ShotPower::MAX); stpInfos["kick_off_taker"].setKickOrChip(KickType::CHIP); + stpInfos["kick_off_taker"].setShootOnFirstTouch(true); } bool KickOffUs::shouldEndPlay() noexcept { diff --git a/roboteam_ai/src/stp/roles/active/BallPlacer.cpp b/roboteam_ai/src/stp/roles/active/BallPlacer.cpp index a92fd9724..4fbfb7a21 100644 --- a/roboteam_ai/src/stp/roles/active/BallPlacer.cpp +++ b/roboteam_ai/src/stp/roles/active/BallPlacer.cpp @@ -10,4 +10,22 @@ namespace rtt::ai::stp::role { BallPlacer::BallPlacer(std::string name) : Role(std::move(name)) { robotTactics = collections::state_machine{tactic::GetBall(), tactic::DriveWithBall(), tactic::BallStandBack()}; } + +Status BallPlacer::update(StpInfo const& info) noexcept { + // Failure if the required data is not present + if (!info.getBall() || !info.getRobot() || !info.getField()) { + RTT_WARNING("Required information missing in the tactic info for ", roleName) + return Status::Failure; + } + + // Stop Get Ball tactic when we have the ball + if (robotTactics.current_num() == 0 && info.getRobot()->get()->hasBall()) { + forceNextTactic(); + } + if (robotTactics.current_num() < 2 && (info.getBall()->get()->position - GameStateManager::getRefereeDesignatedPosition()).length() < constants::BALL_PLACER_MARGIN) { + forceLastTactic(); + } + + return Role::update(info); +} } // namespace rtt::ai::stp::role diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index d9a1411e8..5cc052e63 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -10,6 +10,7 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { auto field = info.getField().value(); auto avoidObj = info.getObjectsToAvoid(); auto targetPos = info.getPositionToMoveTo().value(); + auto targetVel = info.getTargetVelocity(); auto roleName = info.getRoleName(); auto ballLocation = info.getBall()->get()->position; RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); @@ -26,24 +27,31 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT)) { targetPos = PositionComputations::calculateAvoidBallPosition(targetPos, ballLocation, field, avoidObj); } + auto [vel, acc] = info.getCurrentWorld()->getRobotPositionController()->computeAndTrackTrajectory( + info.getCurrentWorld(), field, robot->getId(), robot->getPos(), robot->getVel(), targetPos, targetVel, info.getMaxRobotVelocity(), info.getMaxJerk(), avoidObj); + command.velocity = vel; + command.acceleration = acc; - command.velocity = info.getCurrentWorld()->getRobotPositionController()->computeAndTrackTrajectory(info.getCurrentWorld(), field, robot->getId(), robot->getPos(), - robot->getVel(), targetPos, info.getMaxRobotVelocity(), avoidObj); - - auto distanceToTarget = (robot->getPos() - targetPos).length(); - command.yaw = distanceToTarget <= 0.5 ? info.getYaw() : robot->getYaw() + rtt::Angle(0.5 / distanceToTarget * (info.getYaw() - robot->getYaw())); + command.yaw = info.getYaw(); command.dribblerOn = info.getDribblerOn(); // set command ID command.id = robot->getId(); + if (info.getShootOnFirstTouch() && info.getKickOrChip() != rtt::KickType::NO_KICK && info.getPositionToShootAt()) { + double distanceBallToTarget = (info.getBall()->get()->position - info.getPositionToShootAt().value()).length(); + auto kickChipVelocity = control::ControlUtils::determineKickForce(distanceBallToTarget, info.getShotPower()); + command.kickType = info.getKickOrChip(); + command.kickSpeed = std::clamp(kickChipVelocity, constants::MIN_KICK_POWER, constants::MAX_KICK_POWER); + } + // forward the generated command to the ControlModule, for checking and limiting forwardRobotCommand(); // Check if successful auto distanceError = (robot->getPos() - targetPos).length(); - if (robot->hasBall() && distanceError <= constants::BALL_PLACEMENT_MARGIN - constants::GO_TO_POS_ERROR_MARGIN) { + if ((robot->hasBall() && info.getRoleName() != "ball_placer") || (robot->hasBall() && distanceError <= constants::BALL_PLACER_MARGIN)) { return Status::Success; } else { return Status::Running; diff --git a/roboteam_ai/src/stp/skills/Kick.cpp b/roboteam_ai/src/stp/skills/Kick.cpp index 42b42f84d..c4e057183 100644 --- a/roboteam_ai/src/stp/skills/Kick.cpp +++ b/roboteam_ai/src/stp/skills/Kick.cpp @@ -18,7 +18,7 @@ Status Kick::onUpdate(const StpInfo &info) noexcept { // Set yaw command command.yaw = robot->getYaw(); - command.waitForBall = false; + // command.waitForBall = false; // set command ID command.id = robot->getId(); diff --git a/roboteam_ai/src/stp/skills/OrbitAngular.cpp b/roboteam_ai/src/stp/skills/OrbitAngular.cpp index 36b231d32..9dcfa91fc 100644 --- a/roboteam_ai/src/stp/skills/OrbitAngular.cpp +++ b/roboteam_ai/src/stp/skills/OrbitAngular.cpp @@ -36,7 +36,8 @@ Status OrbitAngular::onUpdate(const StpInfo &info) noexcept { // target yaw is angular velocity times the time step (1/60th of a second) in basestation // in simulator, we multiple by 1/2.5 because of how the simulator works and the pid controller is tuned if (rtt::GameSettings::getRobotHubMode() == rtt::net::RobotHubMode::BASESTATION) { - command.yaw = currentYaw + Angle(targetAngularVelocity * 1 / constants::STP_TICK_RATE); + // TODO ROBOCUP 2024: FIX THIS + command.yaw = currentYaw + Angle(targetAngularVelocity * 1 / 15); } else { command.yaw = currentYaw + Angle(targetAngularVelocity * 1 / 2.5); } diff --git a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp index 7e683b9dc..a82dd3918 100644 --- a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp @@ -38,14 +38,27 @@ std::optional KeeperBlockBall::calculateInfoForSkill(const StpInfo &inf } const auto targetPosition = calculateTargetPosition(info); - skillStpInfo.setPositionToMoveTo(targetPosition.first); - skillStpInfo.setShouldAvoidGoalPosts(targetPosition.second); - skillStpInfo.setShouldAvoidOutOfField(targetPosition.second); - skillStpInfo.setShouldAvoidOurRobots(targetPosition.second); - skillStpInfo.setShouldAvoidTheirRobots(targetPosition.second); + skillStpInfo.setPositionToMoveTo(std::get<0>(targetPosition)); + skillStpInfo.setShouldAvoidGoalPosts(std::get<1>(targetPosition)); + skillStpInfo.setShouldAvoidOutOfField(std::get<1>(targetPosition)); + skillStpInfo.setShouldAvoidOurRobots(std::get<1>(targetPosition)); + skillStpInfo.setShouldAvoidTheirRobots(std::get<1>(targetPosition)); - const auto yaw = calculateYaw(info.getBall().value(), targetPosition.first); - skillStpInfo.setYaw(yaw); + const auto yaw = calculateYaw(info.getBall().value(), std::get<0>(targetPosition)); + + // TODO ROBOCUP 2024: MAKEW MORE PRETTY + const auto &field = info.getField().value(); + const auto &ball = info.getBall().value(); + const auto keepersLineSegment = getKeepersLineSegment(field); + const LineSegment ballTrajectory(ball->position, ball->expectedEndPosition); + bool ballHeadsTowardsOurGoal = ballTrajectory.intersects(keepersLineSegment).has_value(); + if (ballHeadsTowardsOurGoal) { + skillStpInfo.setYaw(skillStpInfo.getRobot()->get()->getYaw()); + } else { + skillStpInfo.setYaw(yaw); + } + + skillStpInfo.setMaxJerk(std::get<2>(targetPosition)); return skillStpInfo; } @@ -55,9 +68,11 @@ bool KeeperBlockBall::isEndTactic() noexcept { return true; } bool KeeperBlockBall::isTacticFailing(const StpInfo &) noexcept { return false; } bool KeeperBlockBall::shouldTacticReset(const StpInfo &info) noexcept { - const double errorMargin = constants::GO_TO_POS_ERROR_MARGIN * M_PI; - const auto distanceToTarget = (info.getRobot().value()->getPos() - info.getPositionToMoveTo().value()).length(); - return distanceToTarget > errorMargin; + // const double errorMargin = constants::GO_TO_POS_ERROR_MARGIN * M_PI; + // const auto distanceToTarget = (info.getRobot().value()->getPos() - info.getPositionToMoveTo().value()).length(); + // return distanceToTarget > errorMargin; + // TODO ROBOCUP 2024: CHECK IF THIS IS LEGIT, or if we do need reset + return false; } const char *KeeperBlockBall::getName() { return "Keeper Block Ball"; } @@ -75,17 +90,22 @@ bool KeeperBlockBall::isBallHeadingTowardsOurGoal(const HalfLine &ballTrajectory return intersectionWithGoalLine.has_value() && goalLineSegment.distanceToLine(intersectionWithGoalLine.value()) < MAX_DISTANCE_HEADING_TOWARDS_GOAL; } -std::pair KeeperBlockBall::calculateTargetPosition(const StpInfo info) noexcept { +std::tuple KeeperBlockBall::calculateTargetPosition(const StpInfo info) noexcept { const auto &field = info.getField().value(); const auto &ball = info.getBall().value(); const auto keepersLineSegment = getKeepersLineSegment(field); const LineSegment ballTrajectory(ball->position, ball->expectedEndPosition); bool ballHeadsTowardsOurGoal = ballTrajectory.intersects(keepersLineSegment).has_value(); + // TODO ROBOCUP 2024: CHECK AND PERHAPS MAKE MORE PRETTY + if (ballHeadsTowardsOurGoal && ballTrajectory.intersects(keepersLineSegment).value().dist(info.getRobot().value()->getPos()) < 0.03) { + return std::make_tuple(info.getRobot().value()->getPos(), false, 12); + } if (ballHeadsTowardsOurGoal) { auto shouldAvoidGoalPosts = false; - return {calculateTargetPositionBallShot(info, keepersLineSegment, ballTrajectory), shouldAvoidGoalPosts}; + auto [targetPosition, maxJerk] = calculateTargetPositionBallShot(info, keepersLineSegment, ballTrajectory); + return std::make_tuple(targetPosition, shouldAvoidGoalPosts, maxJerk); } // This lets the keeper intercept the ball in the defense area when it's not heading towards our goal. This did not work well at the Schubert open, since we always just barely @@ -100,15 +120,15 @@ std::pair KeeperBlockBall::calculateTargetPosition(const StpInfo if (ball->position.x >= field.leftGoalArea.rightLine().center().x - MAX_DISTANCE_BALL_BEHIND_GOAL) { auto shouldAvoidGoalPosts = true; std::optional predictedBallPositionTheirRobot = InterceptionComputations::calculateTheirBallInterception(info.getCurrentWorld(), ballTrajectory); - return {calculateTargetPositionBallNotShot(info, predictedBallPositionTheirRobot), shouldAvoidGoalPosts}; + return {calculateTargetPositionBallNotShot(info, predictedBallPositionTheirRobot), shouldAvoidGoalPosts, ai::constants::MAX_JERK_DEFAULT}; } // Default case: go to the center of the goal auto shouldAvoidGoalPosts = true; - return {Vector2(keepersLineSegment.start.x, 0), shouldAvoidGoalPosts}; + return {Vector2(keepersLineSegment.start.x, 0), shouldAvoidGoalPosts, ai::constants::MAX_JERK_DEFAULT}; } -Vector2 KeeperBlockBall::calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept { +std::pair KeeperBlockBall::calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept { const auto &field = info.getField().value(); const auto &ball = info.getBall().value(); const auto &robot = info.getRobot().value(); @@ -124,12 +144,15 @@ Vector2 KeeperBlockBall::calculateTargetPositionBallShot(const StpInfo info, rtt auto [targetPosition, timeToTarget] = control::OvershootComputations::overshootingDestination(robotPosition, closestPointToGoal.value(), robotVelocity, maxRobotVelocity, maxRobotAcceleration, ballTimeAtClosestPoint); if (timeToTarget <= ballTimeAtClosestPoint) { - return targetPosition; + // TODO ROBOCUP 2024: TWEAK THIS VALUE + auto jerk = (1 - std::min((ballTimeAtClosestPoint - timeToTarget), 0.2) / 0.2) * 80 + ai::constants::MAX_JERK_DEFAULT; + return {targetPosition, jerk}; } } double maxTimeLeftWhenArrived = std::numeric_limits::lowest(); Vector2 optimalTarget = Vector2(); + double jerk = ai::constants::MAX_JERK_DEFAULT; for (double timeStep = 0.01; timeStep <= 3; timeStep += 0.01) { auto predictedBallPosition = FieldComputations::getBallPositionAtTime(*ball.get(), timeStep); @@ -145,9 +168,11 @@ Vector2 KeeperBlockBall::calculateTargetPositionBallShot(const StpInfo info, rtt if (timeLeftWhenArrived > maxTimeLeftWhenArrived) { maxTimeLeftWhenArrived = timeLeftWhenArrived; optimalTarget = currentTarget; + // TODO ROBOCUP 2024: TWEAK THIS VALUE + jerk = (1 - std::min(std::max(timeLeftWhenArrived, 0.0), 0.2) / 0.2) * 80 + ai::constants::MAX_JERK_DEFAULT; } } - return optimalTarget; + return {optimalTarget, jerk}; } Vector2 KeeperBlockBall::calculateTargetPositionBallNotShot(const StpInfo &info, std::optional predictedBallPositionTheirRobot) noexcept { @@ -217,8 +242,8 @@ Vector2 KeeperBlockBall::calculateTargetPositionBallNotShot(const StpInfo &info, Angle KeeperBlockBall::calculateYaw(const world::view::BallView &ball, const Vector2 &targetKeeperPosition) { // Look towards ball to ensure ball hits the front assembly to reduce odds of ball reflecting in goal - const auto keeperToBall = (ball->position - targetKeeperPosition) / 1.6; - return keeperToBall.angle(); + const auto keeperToBall = (ball->position - targetKeeperPosition); + return keeperToBall.angle() / 1.3; } } // namespace rtt::ai::stp::tactic diff --git a/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp b/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp index d51ab3620..4e2d2a96a 100644 --- a/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp @@ -23,7 +23,9 @@ std::optional DriveWithBall::calculateInfoForSkill(const StpInfo &info) bool DriveWithBall::isTacticFailing(const StpInfo &info) noexcept { return !info.getRobot().value()->hasBall() || !info.getPositionToMoveTo(); } bool DriveWithBall::shouldTacticReset(const StpInfo &info) noexcept { - return skills.current_num() == 1 && info.getRobot()->get()->getYaw().shortestAngleDiff(info.getYaw()) > constants::HAS_BALL_ANGLE; + // return skills.current_num() == 1 && info.getRobot()->get()->getYaw().shortestAngleDiff(info.getYaw()) > constants::HAS_BALL_ANGLE; + // TODO ROBOCUP 2024: CHECK IF WE NEED THAT LINE + return false; } bool DriveWithBall::isEndTactic() noexcept { return false; } diff --git a/roboteam_ai/src/stp/tactics/active/GetBall.cpp b/roboteam_ai/src/stp/tactics/active/GetBall.cpp index c3536ecc3..6a48ca794 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -27,14 +27,16 @@ std::optional GetBall::calculateInfoForSkill(const StpInfo &info) noexc Vector2 robotPosition = info.getRobot().value()->getPos(); Vector2 ballPosition = info.getBall().value()->position; - auto maxRobotVelocity = GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); auto interceptionInfo = InterceptionComputations::calculateInterceptionInfo({info.getRobot().value()}, info.getCurrentWorld()); Vector2 interceptionPosition = interceptionInfo.interceptLocation; + Vector2 interceptionVelocity = interceptionInfo.interceptVelocity; - if (info.getRobot()->get()->hasBall()) { - maxRobotVelocity = std::clamp(info.getBall().value()->velocity.length() * 0.8, 0.5, maxRobotVelocity); - skillStpInfo.setMaxRobotVelocity(maxRobotVelocity); - } + // TODO ROBOCUP 2024: CHECK IF NEEDED for ball placer + // auto maxRobotVelocity = GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); + // if (info.getRobot()->get()->hasBall()) { + // maxRobotVelocity = std::clamp(info.getBall().value()->velocity.length() * 0.8, 0.5, maxRobotVelocity); + // skillStpInfo.setMaxRobotVelocity(maxRobotVelocity); + // } double distanceToInterception = (interceptionPosition - robotPosition).length(); double distanceToBall = (ballPosition - robotPosition).length(); @@ -46,14 +48,17 @@ std::optional GetBall::calculateInfoForSkill(const StpInfo &info) noexc // This makes sure we always hit the ball with our front assembly, and we never go to the wrong 'side' of the ball. if (info.getRobot()->get()->getAngleDiffToBall() > constants::HAS_BALL_ANGLE && distanceToBall < constants::ROBOT_CLOSE_TO_POINT) { skillStpInfo.setPositionToMoveTo(info.getRobot()->get()->getPos()); + skillStpInfo.setTargetVelocity(Vector2(0, 0)); } else if (info.getBall()->get()->velocity.length() > constants::BALL_IS_MOVING_SLOW_LIMIT) { auto newRobotPos = interceptionPosition + (interceptionPosition - ballPosition).stretchToLength(constants::CENTER_TO_FRONT); skillStpInfo.setPositionToMoveTo(newRobotPos); + skillStpInfo.setTargetVelocity(interceptionVelocity); } else { auto getBallDistance = std::max(distanceToInterception - constants::CENTER_TO_FRONT, MIN_DISTANCE_TO_TARGET); Vector2 newRobotPosition = robotPosition + (interceptionPosition - robotPosition).stretchToLength(getBallDistance); newRobotPosition = FieldComputations::projectPointToValidPosition(info.getField().value(), newRobotPosition, info.getObjectsToAvoid()); skillStpInfo.setPositionToMoveTo(newRobotPosition); + skillStpInfo.setTargetVelocity(interceptionVelocity); } skillStpInfo.setYaw((ballPosition - robotPosition).angle()); diff --git a/roboteam_ai/src/stp/tactics/active/Receive.cpp b/roboteam_ai/src/stp/tactics/active/Receive.cpp index 770bd3b79..0fe09680a 100644 --- a/roboteam_ai/src/stp/tactics/active/Receive.cpp +++ b/roboteam_ai/src/stp/tactics/active/Receive.cpp @@ -22,8 +22,10 @@ std::optional Receive::calculateInfoForSkill(StpInfo const &info) noexc bool Receive::isTacticFailing(const StpInfo &info) noexcept { return !info.getPositionToMoveTo(); } bool Receive::shouldTacticReset(const StpInfo &info) noexcept { - double errorMargin = constants::GO_TO_POS_ERROR_MARGIN * M_PI; - return (info.getRobot().value()->getPos() - info.getPositionToMoveTo().value()).length() > errorMargin; + // double errorMargin = constants::GO_TO_POS_ERROR_MARGIN * M_PI; + // return (info.getRobot().value()->getPos() - info.getPositionToMoveTo().value()).length() > errorMargin; + // TODO ROBOCUP 2024: CHECK IF NEEDED + return false; } bool Receive::isEndTactic() noexcept { return true; } diff --git a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp index 1edab3627..b05577d59 100644 --- a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp @@ -6,28 +6,38 @@ namespace rtt::ai::stp::tactic { const int STAND_STILL_THRESHOLD = 60; +const int MOVE_AFTER_DRIBBLER_OFF = 20; BallStandBack::BallStandBack() { skills = rtt::collections::state_machine{skill::GoToPos()}; } std::optional BallStandBack::calculateInfoForSkill(StpInfo const &info) noexcept { StpInfo skillStpInfo = info; - if (!info.getPositionToMoveTo().has_value() || !skillStpInfo.getBall().has_value() || !skillStpInfo.getRobot().has_value()) return std::nullopt; + if (!info.getPositionToMoveTo() || !skillStpInfo.getBall() || !skillStpInfo.getRobot()) return std::nullopt; RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); Vector2 targetPosition; Vector2 ballTarget = info.getBall()->get()->position; auto robot = info.getRobot()->get(); if (standStillCounter > STAND_STILL_THRESHOLD) { - auto moveVector = robot->getPos() - ballTarget; - double stretchLength = (currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) ? constants::AVOID_BALL_DISTANCE_BEFORE_FREE_KICK : constants::AVOID_BALL_DISTANCE; - targetPosition = ballTarget + moveVector.stretchToLength(stretchLength); - bool isCloseToTarget = (robot->getPos() - targetPosition).length() < constants::GO_TO_POS_ERROR_MARGIN; - if ((isCloseToTarget || standBack) && currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { - targetPosition = ballTarget + (skillStpInfo.getField().value().leftGoalArea.leftLine().center() - ballTarget).stretchToLength(constants::AVOID_BALL_DISTANCE); - standBack = true; - skillStpInfo.setShouldAvoidBall(true); + skillStpInfo.setDribblerOn(false); + if (standStillCounter > STAND_STILL_THRESHOLD + MOVE_AFTER_DRIBBLER_OFF) { + auto moveVector = robot->getPos() - ballTarget; + double stretchLength = (currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) ? constants::AVOID_BALL_DISTANCE_BEFORE_FREE_KICK : constants::AVOID_BALL_DISTANCE; + targetPosition = ballTarget + moveVector.stretchToLength(stretchLength); + bool isCloseToTarget = (robot->getPos() - targetPosition).length() < constants::GO_TO_POS_ERROR_MARGIN; + if ((isCloseToTarget || standBack) && currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { + targetPosition = ballTarget + (skillStpInfo.getField().value().leftGoalArea.rightLine().center() - ballTarget) + .stretchToLength(constants::AVOID_BALL_DISTANCE + constants::GO_TO_POS_ERROR_MARGIN); + standBack = true; + skillStpInfo.setShouldAvoidBall(true); + } + } else { + standStillCounter++; + targetPosition = robot->getPos(); + skillStpInfo.setShouldAvoidBall(false); } } else { + skillStpInfo.setDribblerOn(true); standBack = false; standStillCounter++; targetPosition = robot->getPos(); @@ -37,14 +47,12 @@ std::optional BallStandBack::calculateInfoForSkill(StpInfo const &info) double yaw = (info.getBall()->get()->position - targetPosition).angle(); skillStpInfo.setPositionToMoveTo(targetPosition); skillStpInfo.setYaw(yaw); - skillStpInfo.setDribblerOn(false); return skillStpInfo; } bool BallStandBack::isTacticFailing(const StpInfo &info) noexcept { - if (!info.getPositionToMoveTo().has_value() || - (info.getBall()->get()->position - GameStateManager::getRefereeDesignatedPosition()).length() > constants::BALL_PLACEMENT_MARGIN) { + if (!info.getPositionToMoveTo() || (info.getBall()->get()->position - GameStateManager::getRefereeDesignatedPosition()).length() > constants::BALL_PLACEMENT_MARGIN) { standStillCounter = 0; return true; } diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index 865f033c1..aee4989fa 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -256,7 +256,7 @@ double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::Ro return 0; } - return costForDistance(robot, *target_position, stpInfo.getMaxRobotVelocity()); + return costForDistance(robot, *target_position, stpInfo.getMaxRobotVelocity(), stpInfo.getMaxJerk()); } double Dealer::getDefaultFlagScores(const v::RobotView &robot, const Dealer::DealerFlag &flag) { @@ -290,8 +290,8 @@ void Dealer::setGameStateRoleIds(std::unordered_map o } // Calculate the cost for distance. The further away the target, the higher the cost for that distance. -double Dealer::costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity) { - return Trajectory2D(robot->getPos(), robot->getVel(), target_position, MaxRobotVelocity, ai::constants::MAX_ACC).getTotalTime(); +double Dealer::costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity, const double MaxRobotJerk) { + return Trajectory2D(robot->getPos(), robot->getVel(), target_position, MaxRobotVelocity, ai::constants::MAX_ACC, MaxRobotJerk, robot->getId()).getTotalTime(); } double Dealer::costForProperty(bool property) { return property ? 0.0 : 1.0; } diff --git a/roboteam_autoref b/roboteam_autoref index b8d5e4d82..d2e7a04b2 160000 --- a/roboteam_autoref +++ b/roboteam_autoref @@ -1 +1 @@ -Subproject commit b8d5e4d82c722080f2a2500dbad3874cd6ce6fc6 +Subproject commit d2e7a04b2979e16a3bb851737b90b6eb89979f75 diff --git a/roboteam_interface/src/app.vue b/roboteam_interface/src/app.vue index ccf26754c..50fdb70cf 100644 --- a/roboteam_interface/src/app.vue +++ b/roboteam_interface/src/app.vue @@ -74,7 +74,7 @@ const bottomPanelSize = computed(() => `${uiStore.panelSize('bottomPanel')}px`)
- {{ key }} : {{ voltage ?? 'N/A' }} + {{ key }} : {{ voltage != null ? Number(voltage).toFixed(2) : 'N/A' }}
diff --git a/roboteam_interface/src/modules/components/panels/feedback-widget.vue b/roboteam_interface/src/modules/components/panels/feedback-widget.vue index 3c9b5c949..671916e7e 100644 --- a/roboteam_interface/src/modules/components/panels/feedback-widget.vue +++ b/roboteam_interface/src/modules/components/panels/feedback-widget.vue @@ -38,7 +38,7 @@ const uiStore = useUIStore() data-tip="Battery level" > - {{ robot.feedbackInfo?.batteryLevel }} + {{ robot.feedbackInfo?.batteryLevel != null ? Number(robot.feedbackInfo.batteryLevel).toFixed(2) : 'N/A' }}
set_dribbler_on(command.dribblerOn); protoCommand->set_wheels_off(command.wheelsOff); + protoCommand->set_acceleration_x(command.acceleration.x); + protoCommand->set_acceleration_y(command.acceleration.y); } return protoCommands; @@ -64,6 +66,7 @@ rtt::RobotCommands protoToRobotCommands(const proto::RobotCommands& protoCommand for (const auto& protoCommand : protoCommands.robot_commands()) { rtt::RobotCommand robotCommand = {.id = protoCommand.id(), .velocity = Vector2(protoCommand.velocity_x(), protoCommand.velocity_y()), + .acceleration = Vector2(protoCommand.acceleration_x(), protoCommand.acceleration_y()), .yaw = protoCommand.yaw(), .targetAngularVelocity = protoCommand.angular_velocity(), .useAngularVelocity = protoCommand.use_angular_velocity(), diff --git a/roboteam_robothub/src/RobotHub.cpp b/roboteam_robothub/src/RobotHub.cpp index 4260a8c9b..9cff22c76 100644 --- a/roboteam_robothub/src/RobotHub.cpp +++ b/roboteam_robothub/src/RobotHub.cpp @@ -154,6 +154,9 @@ void RobotHub::sendCommandsToBasestation(const rtt::RobotCommands &commands, rtt command.rho = static_cast(robotCommand.velocity.length()); command.theta = static_cast(robotCommand.velocity.angle()); + command.acceleration_angle = static_cast(robotCommand.acceleration.angle()); + command.acceleration_magnitude = static_cast(robotCommand.acceleration.length()); + command.useYaw = !robotCommand.useAngularVelocity; command.yaw = static_cast(robotCommand.yaw.getValue()); command.angularVelocity = static_cast(robotCommand.targetAngularVelocity); diff --git a/roboteam_utils/include/roboteam_utils/RobotCommands.hpp b/roboteam_utils/include/roboteam_utils/RobotCommands.hpp index 1388683b0..d21700382 100644 --- a/roboteam_utils/include/roboteam_utils/RobotCommands.hpp +++ b/roboteam_utils/include/roboteam_utils/RobotCommands.hpp @@ -20,6 +20,7 @@ typedef struct RobotCommand { // Positioning related variables Vector2 velocity; // (m/s) Target velocity of the robot + Vector2 acceleration; // (m/s^2) Target acceleration of the robot Angle yaw; // (rad) [-PI, PI] The target yaw of the robot double targetAngularVelocity = 0.0; // (rad/s) The target angular velocity of the robot bool useAngularVelocity = 0.0; // True if angular velocity should be used instead of yaw @@ -29,7 +30,7 @@ typedef struct RobotCommand { // Action related variables double kickSpeed = 0.0; // (m/s) [0, 6.5] The target speed of the ball. Speed of <= 0.0 is undefined - bool waitForBall = false; // Will make the robot wait with kicking until it has the ball + bool waitForBall = true; // Will make the robot wait with kicking until it has the ball KickType kickType = KickType::NO_KICK; // Defines the type of kicking, either normal(horizontal) or chipping(vertical), or no kick bool kickAtYaw = false; // Makes robot kick once it arrives at the specified yaw, used in combination with angular velocity