From 893ad380a15fb4e75127df98411bcd3c59fb2803 Mon Sep 17 00:00:00 2001 From: Jorn Date: Thu, 8 Feb 2024 18:06:23 +0100 Subject: [PATCH 01/22] Refactor position computations to avoid robots --- .../stp/computations/PositionComputations.h | 12 ++-- .../stp/computations/PositionComputations.cpp | 64 ++++++++++++++----- roboteam_ai/src/stp/skills/GoToPos.cpp | 3 + 3 files changed, 57 insertions(+), 22 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h index 856f3f512..ed5e402b6 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h @@ -70,13 +70,15 @@ class PositionComputations { static Vector2 getWallPosition(int index, int amountDefenders, const Field &field, world::World *world); /** - * @brief Calculates where a robot should stand to prevent the ball from going in the goal - * @param field The current field + * @brief Calculates the position of the robot to avoid other robots + * @param targetPosition The initial target position * @param world The current world - * @return The position a robot should go to to block the ball (this does not depend on the position of any of our robots) + * @param robotId The id of the robot + * @param avoidObj The objects to avoid + * @param field The current field + * @return A position that is not too close to other robots */ - static Vector2 getBallBlockPosition(const Field &field, const world::World *world); - + static Vector2 calculateAvoidRobotsPosition(Vector2 targetPosition, const world::World *world, int robotId, const AvoidObjects &avoidObj, const Field &field); /** * @brief Calculates a position, near the target position, that is not too close to the ball * @param targetPosition The initial target position diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index b02fafdd5..ed0c2d36b 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -171,17 +171,56 @@ std::vector PositionComputations::determineWallPositions(const rtt::Fie } return positions; } + +Vector2 PositionComputations::calculateAvoidRobotsPosition(Vector2 targetPosition, const world::World *world, int robotId, const AvoidObjects &avoidObj, const Field &field) { + std::vector pointsToAvoid = {}; + if (avoidObj.shouldAvoidOurRobots) { + for (auto &robot : world->getWorld()->getUs()) { + if (robot->getId() != robotId) { + pointsToAvoid.push_back(robot->getPos()); + } + } + } + if (avoidObj.shouldAvoidTheirRobots || avoidObj.notAvoidTheirRobotId != -1) { + for (auto &robot : world->getWorld()->getThem()) { + if (robot->getId() != avoidObj.notAvoidTheirRobotId) { + pointsToAvoid.push_back(robot->getPos()); + } + } + } + + if (std::all_of(pointsToAvoid.begin(), pointsToAvoid.end(), + [&](const Vector2 &avoidPoint) { return avoidPoint.dist(targetPosition) >= 2 * control_constants::ROBOT_RADIUS; })) { + return targetPosition; + } + + for (int distanceSteps = 0; distanceSteps < 5; ++distanceSteps) { + auto distance = 4 * control_constants::ROBOT_RADIUS + distanceSteps * control_constants::ROBOT_RADIUS * 2; + auto possiblePoints = Grid(targetPosition.x - distance / 2.0, targetPosition.y - distance / 2.0, distance, distance, 3, 3).getPoints(); + for (auto &pointVector : possiblePoints) { + for (auto &point : pointVector) { + if (FieldComputations::pointIsValidPosition(field, point) && std::all_of(pointsToAvoid.begin(), pointsToAvoid.end(), [&](const Vector2 &avoidPoint) { + return avoidPoint.dist(point) >= 2 * control_constants::ROBOT_RADIUS; + })) { + return point; + } + } + } + } + RTT_WARNING("Could not find good position to avoid robots for robot with id: " + std::to_string(robotId)); + return targetPosition; +} + Vector2 PositionComputations::calculateAvoidBallPosition(Vector2 targetPosition, Vector2 ballPosition, const Field &field) { RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); std::unique_ptr avoidShape; - // During ball placement, we need to avoid the area between the ball and the target position by a certain margin if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { - avoidShape = std::make_unique(Tube(ballPosition, GameStateManager::getRefereeDesignatedPosition(), control_constants::AVOID_BALL_DISTANCE + 0.1)); + avoidShape = std::make_unique( + Tube(ballPosition, GameStateManager::getRefereeDesignatedPosition(), control_constants::AVOID_BALL_DISTANCE + control_constants::GO_TO_POS_ERROR_MARGIN)); } else { - // During stop gamestate, we need to avoid the area directly around the ball. - avoidShape = std::make_unique(Circle(ballPosition, control_constants::AVOID_BALL_DISTANCE)); + avoidShape = std::make_unique(Circle(ballPosition, control_constants::AVOID_BALL_DISTANCE + control_constants::GO_TO_POS_ERROR_MARGIN)); } if (avoidShape->contains(targetPosition)) { @@ -196,28 +235,19 @@ Vector2 PositionComputations::calculateAvoidBallPosition(Vector2 targetPosition, } Vector2 PositionComputations::calculatePositionOutsideOfShape(Vector2 targetPosition, const rtt::Field &field, const std::unique_ptr &avoidShape) { - Vector2 newTarget = targetPosition; // The new position to go to - bool pointFound = false; for (int distanceSteps = 0; distanceSteps < 5; ++distanceSteps) { - // Use a larger grid each iteration in case no valid point is found - auto distance = 3 * control_constants::AVOID_BALL_DISTANCE + distanceSteps * control_constants::AVOID_BALL_DISTANCE / 2.0; + auto distance = 2 * control_constants::AVOID_BALL_DISTANCE + distanceSteps * control_constants::AVOID_BALL_DISTANCE; auto possiblePoints = Grid(targetPosition.x - distance / 2.0, targetPosition.y - distance / 2.0, distance, distance, 3, 3).getPoints(); - double dist = 1e3; for (auto &pointVector : possiblePoints) { for (auto &point : pointVector) { if (FieldComputations::pointIsValidPosition(field, point) && !avoidShape->contains(point)) { - if (targetPosition.dist(point) < dist) { - dist = targetPosition.dist(point); - newTarget = point; - pointFound = true; - } + return point; } } } - if (pointFound) break; // As soon as a valid point is found, don't look at more points further away } - if (newTarget == targetPosition) RTT_WARNING("Could not find good position to avoid ball"); - return newTarget; + RTT_WARNING("Could not find good position to avoid ball"); + return targetPosition; } void PositionComputations::calculateInfoForKeeper(std::unordered_map &stpInfos, const Field &field, world::World *world) noexcept { diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index adfb1f595..804455aa7 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -18,6 +18,9 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { if (!FieldComputations::pointIsValidPosition(info.getField().value(), targetPos, avoidObj) && roleName != "ball_placer") { targetPos = FieldComputations::projectPointToValidPosition(info.getField().value(), targetPos, avoidObj); } + if (avoidObj.shouldAvoidOurRobots || avoidObj.shouldAvoidTheirRobots || avoidObj.notAvoidTheirRobotId != -1) { + targetPos = PositionComputations::calculateAvoidRobotsPosition(targetPos, info.getCurrentWorld(), info.getRobot().value()->getId(), avoidObj, info.getField().value()); + } if (avoidObj.shouldAvoidBall) { targetPos = PositionComputations::calculateAvoidBallPosition(targetPos, ballLocation, info.getField().value()); } From 708e410759c5f78ab14ad901c2f5d615b50db841 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 9 Feb 2024 10:56:56 +0100 Subject: [PATCH 02/22] - Change big numbers to numeric limits - Fix intermediate points location (was mirrored), also made them slightly unsymetrical) - For defenders, robots closest to placement location during ball placement them instead of clostest to ball --- .../src/control/positionControl/PositionControl.cpp | 8 +++++--- roboteam_ai/src/world/views/WorldDataView.cpp | 4 ++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index a8703286c..67142ac50 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -164,7 +164,7 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: [&targetPosition](const Vector2 &a, const Vector2 &b) { return (a - targetPosition).length() < (b - targetPosition).length(); }); timeStep *= 3; - double bestScore = 999; + double bestScore = std::numeric_limits::max(); std::optional bestTrajectory = std::nullopt; for (const auto &intermediatePoint : intermediatePoints) { @@ -198,7 +198,7 @@ std::vector PositionControl::createIntermediatePoints(const rtt::Field float angleBetweenIntermediatePoints = M_PI_4 / 2; float fieldHeight = field.playArea.height(); float pointExtension = fieldHeight / 18; - Vector2 collisionToTargetNormalized = (firstCollision->collisionPosition - targetPosition).normalize(); + Vector2 collisionToTargetNormalized = (targetPosition - firstCollision->collisionPosition).normalize(); Vector2 pointToDrawFrom = firstCollision->collisionPosition + collisionToTargetNormalized * pointExtension; std::vector intermediatePoints; @@ -206,7 +206,9 @@ std::vector PositionControl::createIntermediatePoints(const rtt::Field Vector2 pointToRotate = pointToDrawFrom + collisionToTargetNormalized * intermediatePointRadius; for (int i = -6; i < 7; i++) { if (i != 0) { - Vector2 intermediatePoint = pointToRotate.rotateAroundPoint(i * angleBetweenIntermediatePoints, pointToDrawFrom); + // The slight offset to the angle makes sure the points are not symmetrically placed, this means we don't keep on switching between two points that are equally good + // when there is a collision at time 0ss + Vector2 intermediatePoint = pointToRotate.rotateAroundPoint(i * angleBetweenIntermediatePoints - 0.01, pointToDrawFrom); if (field.playArea.contains(intermediatePoint)) { intermediatePoints.emplace_back(intermediatePoint); diff --git a/roboteam_ai/src/world/views/WorldDataView.cpp b/roboteam_ai/src/world/views/WorldDataView.cpp index 1c5888328..c0c516530 100644 --- a/roboteam_ai/src/world/views/WorldDataView.cpp +++ b/roboteam_ai/src/world/views/WorldDataView.cpp @@ -60,7 +60,7 @@ std::optional WorldDataView::whichRobotHasBall(Team team) const { robots = getRobotsNonOwning(); } - double bestDistance = 9e9; + double bestDistance = std::numeric_limits::max(); RobotView bestRobot = RobotView{nullptr}; for (auto &robot : robots) { if (robot->hasBall()) { @@ -79,7 +79,7 @@ std::optional WorldDataView::getRobotClosestToPoint(const Vector2 &po if (robots.empty()) return std::nullopt; size_t bestIndex = 0; - double closest = 9e9; + double closest = std::numeric_limits::max(); for (size_t i = 0; i < robots.size(); i++) { if (auto currentBot = robots[i]) { double distance = (currentBot->getPos() - point).length(); From beee4d1fc9fc2ded50096770358b6b7bd40eab8d Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 9 Feb 2024 11:03:28 +0100 Subject: [PATCH 03/22] Ignore placement location robot instead of closest to ball --- .../src/stp/computations/PositionComputations.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index ed0c2d36b..6db8870de 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -307,8 +307,13 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma } // Calculate the n closest enemies auto enemyRobots = world->getWorld()->getThem(); - auto enemyClosestToBall = world->getWorld()->getRobotClosestToBall(world::them); - erase_if(enemyRobots, [&](const auto enemyRobot) -> bool { return enemyClosestToBall && enemyRobot->getId() == enemyClosestToBall.value()->getId(); }); + std::optional enemyToIgnore; + if (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM) { + enemyToIgnore = world->getWorld()->getRobotClosestToPoint(GameStateManager::getRefereeDesignatedPosition(), world::them); + } else { + enemyToIgnore = world->getWorld()->getRobotClosestToBall(world::them); + } + erase_if(enemyRobots, [&](const auto enemyRobot) -> bool { return enemyToIgnore && enemyRobot->getId() == enemyToIgnore.value()->getId(); }); std::map enemyMap; std::vector enemies; for (auto enemy : enemyRobots) { From 80c365adf9900b8a565ea6fb0abd8a037d458205 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 9 Feb 2024 13:58:02 +0100 Subject: [PATCH 04/22] Make sure ball placer stops when ball is close enough to the target. --- roboteam_ai/include/roboteam_ai/stp/Role.hpp | 5 +++++ .../include/roboteam_ai/stp/constants/ControlConstants.h | 6 +++--- roboteam_ai/src/stp/Role.cpp | 2 ++ .../plays/referee_specific/BallPlacementUsForceStart.cpp | 4 ++++ .../stp/plays/referee_specific/BallPlacementUsFreeKick.cpp | 4 ++++ roboteam_ai/src/stp/skills/GoToPos.cpp | 3 ++- 6 files changed, 20 insertions(+), 4 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/Role.hpp b/roboteam_ai/include/roboteam_ai/stp/Role.hpp index f93ad3759..aca276d45 100644 --- a/roboteam_ai/include/roboteam_ai/stp/Role.hpp +++ b/roboteam_ai/include/roboteam_ai/stp/Role.hpp @@ -59,6 +59,11 @@ class Role { */ void forceNextTactic() noexcept; + /** + * @brief Forces the Role to skip to the last tactic in the state machine + */ + void forceLastTactic() noexcept; + /** * @brief Resets the tactics, skills and robot of this role so re-dealing of robots works as expected */ diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index a1af3ea9f..b40b8b145 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -52,9 +52,9 @@ constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot // Angle margin for 'goToPos'. If the robot is within this margin, goToPos is successful constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.04; /**< Angle error for a robot to be considered to have reached a position */ // Maximum inaccuracy during ballplacement -constexpr double BALL_PLACEMENT_MARGIN = 0.15; /**< Distance error for the ball to be considered to have reached the ball placement position*/ -constexpr double DEALER_SPEED_FACTOR = 0.5; /**< Multiplication factor of speed used by the dealer */ -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 */ +constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS; /**< Distance error for the ball to be considered to have reached the ball placement position*/ +constexpr double DEALER_SPEED_FACTOR = 0.5; /**< Multiplication factor of speed used by the dealer */ +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 constexpr uint8_t FUZZY_TRUE = 255; /**< Value at which the fuzzy logic is considered 100% true */ diff --git a/roboteam_ai/src/stp/Role.cpp b/roboteam_ai/src/stp/Role.cpp index a10d54ecc..a57586b69 100644 --- a/roboteam_ai/src/stp/Role.cpp +++ b/roboteam_ai/src/stp/Role.cpp @@ -52,6 +52,8 @@ bool Role::finished() const noexcept { return robotTactics.finished(); } void Role::forceNextTactic() noexcept { robotTactics.skip_n(1); } +void Role::forceLastTactic() noexcept { robotTactics.skip_to(robotTactics.size() - 1); } + std::optional const& Role::getCurrentRobot() const { return currentRobot; } Tactic* Role::getCurrentTactic() { return robotTactics.get_current(); } diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp index cf11e624e..f64f594ac 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp @@ -88,6 +88,10 @@ void BallPlacementUsForceStart::calculateInfoForRoles() noexcept { for (auto& stpInfo : stpInfos) { stpInfo.second.setShouldAvoidDefenseArea(false); } + if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < control_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()->getDistanceToBall() < control_constants::TURN_ON_DRIBBLER_DISTANCE) { diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp index 0a727597f..703f91b31 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp @@ -89,6 +89,10 @@ void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { stpInfo.second.setShouldAvoidDefenseArea(false); } + if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < control_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()->getDistanceToBall() < control_constants::TURN_ON_DRIBBLER_DISTANCE) { stpInfos["ball_placer"].setDribblerSpeed(100); diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index 804455aa7..a1ab99e29 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -58,7 +58,8 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { // Check if successful if ((info.getRobot().value()->getPos() - targetPos).length() <= stp::control_constants::GO_TO_POS_ERROR_MARGIN || - (info.getRobot().value()->hasBall() && (info.getRobot().value()->getPos() - targetPos).length() <= stp::control_constants::BALL_PLACEMENT_MARGIN)) { + (info.getRobot().value()->hasBall() && + (info.getRobot().value()->getPos() - targetPos).length() <= stp::control_constants::BALL_PLACEMENT_MARGIN - stp::control_constants::GO_TO_POS_ERROR_MARGIN)) { return Status::Success; } else { return Status::Running; From ba0969d9ddc3e06591799061b21bf25a8143a7d0 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 9 Feb 2024 17:28:19 +0100 Subject: [PATCH 05/22] Fix ball teleporting when vision loses the ball and update expected end position --- .../include/roboteam_ai/world/Ball.hpp | 7 +++--- roboteam_ai/src/utilities/StrategyManager.cpp | 4 ++++ roboteam_ai/src/world/Ball.cpp | 24 +++++++++++++++---- roboteam_ai/src/world/WorldData.cpp | 10 ++++---- roboteam_ai/src/world/views/WorldDataView.cpp | 3 +++ 5 files changed, 36 insertions(+), 12 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/world/Ball.hpp b/roboteam_ai/include/roboteam_ai/world/Ball.hpp index 1a886b3f6..6c980e859 100644 --- a/roboteam_ai/include/roboteam_ai/world/Ball.hpp +++ b/roboteam_ai/include/roboteam_ai/world/Ball.hpp @@ -20,9 +20,10 @@ namespace rtt::world::ball { */ class Ball { public: - Vector2 position; /**< Position of the ball */ - Vector2 velocity; /**< Velocity of the ball */ - bool visible = false; /**< Whether the ball is visible by any camera */ + Vector2 position; /**< Position of the ball */ + Vector2 velocity; /**< Velocity of the ball */ + Vector2 expectedEndPosition; /**< Expected end position of the ball if it keeps moving in the same direction */ + bool visible = false; /**< Whether the ball is visible by any camera */ /** * @brief Initializes ball at the previously seen position, if the current ball is not visible diff --git a/roboteam_ai/src/utilities/StrategyManager.cpp b/roboteam_ai/src/utilities/StrategyManager.cpp index d5cd03de2..958407ed2 100644 --- a/roboteam_ai/src/utilities/StrategyManager.cpp +++ b/roboteam_ai/src/utilities/StrategyManager.cpp @@ -13,6 +13,10 @@ void StrategyManager::setCurrentGameState(RefCommand command, RefCommand nextCom command = nextCommand; } + if (command == RefCommand::FORCED_START) { + command = RefCommand::NORMAL_START; + } + if (command == RefCommand::BALL_PLACEMENT_US && nextCommand == RefCommand::DIRECT_FREE_US) { command = RefCommand::BALL_PLACEMENT_US_DIRECT; } diff --git a/roboteam_ai/src/world/Ball.cpp b/roboteam_ai/src/world/Ball.cpp index 54415863b..8915f01b4 100644 --- a/roboteam_ai/src/world/Ball.cpp +++ b/roboteam_ai/src/world/Ball.cpp @@ -4,6 +4,7 @@ #include "world/Ball.hpp" +#include "gui/Out.h" #include "utilities/Constants.h" #include "utilities/GameSettings.h" #include "world/World.hpp" @@ -16,7 +17,7 @@ namespace rtt::world::ball { * * The expected movement friction of the ball during simulation */ -constexpr static float SIMULATION_FRICTION = 1.22; +constexpr static float SIMULATION_FRICTION = 0.69; /** * The expected movement friction of the ball on the field @@ -26,9 +27,9 @@ constexpr static float REAL_FRICTION = 0.5; Ball::Ball(const proto::WorldBall& copy, const World* data) : position{copy.pos().x(), copy.pos().y()}, velocity{copy.vel().x(), copy.vel().y()}, visible{copy.visible()} { if (!visible || position == Vector2()) { initBallAtExpectedPosition(data); - updateBallAtRobotPosition(data); } - updateExpectedBallEndPosition(data); + updateBallAtRobotPosition(data); + // updateExpectedBallEndPosition(data); } void Ball::initBallAtExpectedPosition(const world::World* data) noexcept { @@ -52,7 +53,19 @@ void Ball::updateExpectedBallEndPosition(const world::World* data) noexcept { double ballVelSquared = ball->velocity.length2(); const double frictionCoefficient = GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? SIMULATION_FRICTION : REAL_FRICTION; - ball->position + ball->velocity.stretchToLength(ballVelSquared / frictionCoefficient); + Vector2 expectedEnd = ball->position + ball->velocity.stretchToLength(ballVelSquared / frictionCoefficient); + std::array arr = {expectedEnd, ball->position}; + std::span span(arr); + rtt::ai::gui::Out::draw( + { + .label = "expectedBallEnd", + .color = proto::Drawing::MAGENTA, + .method = proto::Drawing::LINES_CONNECTED, + .category = proto::Drawing::PATH_PLANNING, + .forRobotId = 3, + .thickness = 3, + }, + span); } void Ball::updateBallAtRobotPosition(const world::World* data) noexcept { @@ -63,6 +76,9 @@ void Ball::updateBallAtRobotPosition(const world::World* data) noexcept { if (!robotWithBall.has_value()) { return; } + if ((robotWithBall->get()->getVel() - velocity).length() > 0.1) { + return; + } // Place the ball where we would expect it to be given that this robot has the ball double distanceInFrontOfRobot = ai::stp::control_constants::CENTER_TO_FRONT + ai::Constants::BALL_RADIUS(); position = robotWithBall->get()->getPos() + robotWithBall->get()->getAngle().toVector2(distanceInFrontOfRobot); diff --git a/roboteam_ai/src/world/WorldData.cpp b/roboteam_ai/src/world/WorldData.cpp index ce4d75f40..16c5b5e5d 100644 --- a/roboteam_ai/src/world/WorldData.cpp +++ b/roboteam_ai/src/world/WorldData.cpp @@ -50,11 +50,11 @@ WorldData::WorldData(const World *data, proto::World &protoMsg) noexcept : time{ setViewVectors(); // TODO: add information from robots which were only seen on feedback but not on vision - if (GameSettings::isYellow() && protoMsg.yellow_unseen_robots_size() > 0) { - RTT_WARNING("Received feedback from unseen robots!") - } else if (!GameSettings::isYellow() && protoMsg.blue_unseen_robots_size() > 0) { - RTT_WARNING("Received feedback from unseen robots!") - } + // if (GameSettings::isYellow() && protoMsg.yellow_unseen_robots_size() > 0) { + // RTT_WARNING("Received feedback from unseen robots!") + // } else if (!GameSettings::isYellow() && protoMsg.blue_unseen_robots_size() > 0) { + // RTT_WARNING("Received feedback from unseen robots!") + // } } std::vector const &WorldData::getUs() const noexcept { return us; } diff --git a/roboteam_ai/src/world/views/WorldDataView.cpp b/roboteam_ai/src/world/views/WorldDataView.cpp index c0c516530..1a8c86d44 100644 --- a/roboteam_ai/src/world/views/WorldDataView.cpp +++ b/roboteam_ai/src/world/views/WorldDataView.cpp @@ -65,6 +65,9 @@ std::optional WorldDataView::whichRobotHasBall(Team team) const { for (auto &robot : robots) { if (robot->hasBall()) { auto distanceToBall = robot->getDistanceToBall(); + if (robot->getTeam() == us) { + distanceToBall -= 99; + } if (distanceToBall < bestDistance) { bestDistance = distanceToBall; bestRobot = robot; From dbd7f25c0ba6c28076c484a9c4e8b2461d4d2e90 Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 12 Feb 2024 11:25:23 +0100 Subject: [PATCH 06/22] Use expected end position for the ball during deciding when keeper should kick and where harasher should go --- .../src/stp/computations/PositionComputations.cpp | 11 ++++++++--- .../BallInOurDefenseAreaAndStillGlobalEvaluation.cpp | 2 +- ...allNotInOurDefenseAreaAndStillGlobalEvaluation.cpp | 2 +- roboteam_ai/src/stp/tactics/active/GetBall.cpp | 7 ++++++- roboteam_ai/src/world/Ball.cpp | 11 +++++------ 5 files changed, 21 insertions(+), 12 deletions(-) diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 6db8870de..bee33246a 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -258,13 +258,18 @@ void PositionComputations::calculateInfoForKeeper(std::unordered_map &stpInfos, std::array, stp::control_constants::MAX_ROBOT_COUNT> *roles, const Field &field, world::World *world) noexcept { - auto enemyClosestToBall = world->getWorld()->getRobotClosestToBall(world::them); + rtt::Vector2 ballPos = world->getWorld()->getBall()->get()->position; + if (field.leftDefenseArea.contains(ballPos)) { + std::vector intersections = + FieldComputations::getDefenseArea(field, true, 0, 0).intersections({ballPos, world->getWorld()->getBall()->get()->expectedEndPosition}); + if (intersections.size() == 1) ballPos = intersections.at(0); + } + auto enemyClosestToBall = world->getWorld()->getRobotClosestToPoint(ballPos, world::them); // If there is no enemy or we don't have a harasser yet, estimate the position to move to if (!stpInfos["harasser"].getRobot() || !enemyClosestToBall) { - stpInfos["harasser"].setPositionToMoveTo(world->getWorld()->getBall()->get()->position); + stpInfos["harasser"].setPositionToMoveTo(ballPos); return; } - auto ballPos = world->getWorld()->getBall()->get()->position; auto enemyAngle = enemyClosestToBall->get()->getAngle(); auto enemyToGoalAngle = (field.leftGoalArea.leftLine().center() - enemyClosestToBall->get()->getPos()).angle(); diff --git a/roboteam_ai/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp index c4640df03..b76f27c9f 100644 --- a/roboteam_ai/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp @@ -11,7 +11,7 @@ namespace rtt::ai::stp::evaluation { uint8_t BallInOurDefenseAreaAndStillGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { return (field->leftDefenseArea.contains(world->getWorld()->getBall()->get()->position) && - world->getWorld()->getBall()->get()->velocity.length() < control_constants::BALL_STILL_VEL) + field->leftDefenseArea.contains(world->getWorld()->getBall()->get()->expectedEndPosition)) ? control_constants::FUZZY_TRUE : control_constants::FUZZY_FALSE; } diff --git a/roboteam_ai/src/stp/evaluations/global/BallNotInOurDefenseAreaAndStillGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/BallNotInOurDefenseAreaAndStillGlobalEvaluation.cpp index afd7bf5ad..7df20e75c 100644 --- a/roboteam_ai/src/stp/evaluations/global/BallNotInOurDefenseAreaAndStillGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/BallNotInOurDefenseAreaAndStillGlobalEvaluation.cpp @@ -11,7 +11,7 @@ namespace rtt::ai::stp::evaluation { uint8_t BallNotInOurDefenseAreaAndStillGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { return (field->leftDefenseArea.contains(world->getWorld()->getBall()->get()->position) && - world->getWorld()->getBall()->get()->velocity.length() < control_constants::BALL_STILL_VEL) + field->leftDefenseArea.contains(world->getWorld()->getBall()->get()->expectedEndPosition)) ? control_constants::FUZZY_FALSE : control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/tactics/active/GetBall.cpp b/roboteam_ai/src/stp/tactics/active/GetBall.cpp index 1b5c21b3c..ed14aae89 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -18,10 +18,15 @@ GetBall::GetBall() { skills = collections::state_machine std::optional GetBall::calculateInfoForSkill(StpInfo const &info) noexcept { StpInfo skillStpInfo = info; - if (!skillStpInfo.getRobot() || !skillStpInfo.getBall()) return std::nullopt; + if (!skillStpInfo.getRobot() || !skillStpInfo.getBall() || !skillStpInfo.getField()) return std::nullopt; Vector2 robotPosition = skillStpInfo.getRobot().value()->getPos(); Vector2 ballPosition = skillStpInfo.getBall().value()->position; + if (skillStpInfo.getField().value().leftDefenseArea.contains(ballPosition)) { + std::vector intersections = + FieldComputations::getDefenseArea(skillStpInfo.getField().value(), true, 0, 0).intersections({ballPosition, skillStpInfo.getBall().value()->expectedEndPosition}); + if (intersections.size() == 1) ballPosition = intersections.at(0); + } double ballDistance = (ballPosition - robotPosition).length() - control_constants::BALL_RADIUS - control_constants::ROBOT_RADIUS + control_constants::GO_TO_POS_ERROR_MARGIN + 2 * control_constants::BALL_RADIUS; diff --git a/roboteam_ai/src/world/Ball.cpp b/roboteam_ai/src/world/Ball.cpp index 8915f01b4..967c4c7bd 100644 --- a/roboteam_ai/src/world/Ball.cpp +++ b/roboteam_ai/src/world/Ball.cpp @@ -29,7 +29,7 @@ Ball::Ball(const proto::WorldBall& copy, const World* data) : position{copy.pos( initBallAtExpectedPosition(data); } updateBallAtRobotPosition(data); - // updateExpectedBallEndPosition(data); + updateExpectedBallEndPosition(data); } void Ball::initBallAtExpectedPosition(const world::World* data) noexcept { @@ -53,16 +53,15 @@ void Ball::updateExpectedBallEndPosition(const world::World* data) noexcept { double ballVelSquared = ball->velocity.length2(); const double frictionCoefficient = GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? SIMULATION_FRICTION : REAL_FRICTION; - Vector2 expectedEnd = ball->position + ball->velocity.stretchToLength(ballVelSquared / frictionCoefficient); - std::array arr = {expectedEnd, ball->position}; + expectedEndPosition = ball->position + ball->velocity.stretchToLength(ballVelSquared / frictionCoefficient); + std::array arr = {expectedEndPosition, ball->position}; std::span span(arr); + // maybe change to a cross at the end instead of a line? rtt::ai::gui::Out::draw( { - .label = "expectedBallEnd", + .label = "expected_end_position_ball", .color = proto::Drawing::MAGENTA, .method = proto::Drawing::LINES_CONNECTED, - .category = proto::Drawing::PATH_PLANNING, - .forRobotId = 3, .thickness = 3, }, span); From cb3cd768b541362f022b85493c482820e8c6b738 Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 12 Feb 2024 17:12:35 +0100 Subject: [PATCH 07/22] Use expected end position when ball is in opponents defense area --- roboteam_ai/src/stp/tactics/active/GetBall.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/roboteam_ai/src/stp/tactics/active/GetBall.cpp b/roboteam_ai/src/stp/tactics/active/GetBall.cpp index ed14aae89..fc9f4ea55 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -26,6 +26,10 @@ std::optional GetBall::calculateInfoForSkill(StpInfo const &info) noexc std::vector intersections = FieldComputations::getDefenseArea(skillStpInfo.getField().value(), true, 0, 0).intersections({ballPosition, skillStpInfo.getBall().value()->expectedEndPosition}); if (intersections.size() == 1) ballPosition = intersections.at(0); + } else if (skillStpInfo.getField().value().rightDefenseArea.contains(ballPosition)) { + std::vector intersections = + FieldComputations::getDefenseArea(skillStpInfo.getField().value(), false, 0, 0).intersections({ballPosition, skillStpInfo.getBall().value()->expectedEndPosition}); + if (intersections.size() == 1) ballPosition = intersections.at(0); } double ballDistance = (ballPosition - robotPosition).length() - control_constants::BALL_RADIUS - control_constants::ROBOT_RADIUS + control_constants::GO_TO_POS_ERROR_MARGIN + 2 * control_constants::BALL_RADIUS; From 3b86a7ee5039e642a3f926dded7fe07c6d74b9d5 Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 12 Feb 2024 17:13:15 +0100 Subject: [PATCH 08/22] Use expected end position when ball is in opponents defense area but now all the files are commited --- roboteam_ai/src/control/positionControl/PositionControl.cpp | 6 ++++++ roboteam_ai/src/stp/computations/PositionComputations.cpp | 4 ++++ 2 files changed, 10 insertions(+) diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 67142ac50..eecc896c7 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -60,6 +60,12 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: firstCollision = worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects, completedTimeSteps); if (firstCollision.has_value()) { commandCollision.collisionData = firstCollision; + if ((firstCollision.value().collisionType == BB::CollisionType::BALL || firstCollision.value().collisionType == BB::CollisionType::BALLPLACEMENT) && + firstCollision.value().collisionTime <= 0.11) { + targetPosition = + currentPosition + (currentPosition - world->getWorld()->getBall()->get()->position).stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); + } } } diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index bee33246a..4cad2ccbc 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -263,6 +263,10 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_map intersections = FieldComputations::getDefenseArea(field, true, 0, 0).intersections({ballPos, world->getWorld()->getBall()->get()->expectedEndPosition}); if (intersections.size() == 1) ballPos = intersections.at(0); + } else if (field.rightDefenseArea.contains(ballPos)) { + std::vector intersections = + FieldComputations::getDefenseArea(field, false, 0, 0).intersections({ballPos, world->getWorld()->getBall()->get()->expectedEndPosition}); + if (intersections.size() == 1) ballPos = intersections.at(0); } auto enemyClosestToBall = world->getWorld()->getRobotClosestToPoint(ballPos, world::them); // If there is no enemy or we don't have a harasser yet, estimate the position to move to From 67fc9cc47af24c30ba9abdfc17f0f158b80efa24 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 13 Feb 2024 17:11:59 +0100 Subject: [PATCH 09/22] Add collision handling for ball and ball placement collision at time 0. We will drive as quickly away as possible from the avoidance area (ball or placement area) --- .../control/positionControl/PositionControl.h | 20 ++++++ .../BBTrajectories/WorldObjects.cpp | 6 +- .../positionControl/PositionControl.cpp | 64 +++++++++++++++++-- 3 files changed, 82 insertions(+), 8 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index d68d2ea66..12a10af8d 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -105,6 +105,26 @@ class PositionControl { rtt::BB::CommandCollision computeAndTrackTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity, Vector2 targetPosition, double maxRobotVelocity, stp::PIDType pidType, 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. + * @param world a pointer to the current world + * @param field the field object, used onwards by the collision detector + * @param currentPosition the current position of the robot + * @param avoidObjects whether or not to avoid objects + * @return A new trajectory to the target + */ + Vector2 handleBallCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects); + + /** + * @brief Handles the collision with the ball placement at the current position, will move away from the ball placement location to ball line as quickly as possible. + * @param world a pointer to the current world + * @param field the field object, used onwards by the collision detector + * @param currentPosition the current position of the robot + * @param avoidObjects whether or not to avoid objects + * @return A new trajectory to the target + */ + Vector2 handleBallPlacementCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects); + /** * @brief Calculates a score for a trajectory. The score is based on the distance to the target and the * distance to the first collision on the path diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 00ada1746..0b0702124 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -27,6 +27,9 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W if (avoidObjects.shouldAvoidDefenseArea) { calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); } + if (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM) { + calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); + } if (avoidObjects.shouldAvoidBall) { calculateBallCollisions(world, collisionDatas, pathPoints, timeStep, avoidObjects.avoidBallDist, completedTimeSteps[robotId], startTime); } @@ -36,9 +39,6 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W if (avoidObjects.shouldAvoidOurRobots) { calculateOurRobotCollisions(world, collisionDatas, pathPoints, computedPaths, robotId, timeStep, completedTimeSteps, startTime); } - if (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM) { - calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); - } if (!collisionDatas.empty()) { return collisionDatas[0]; } else { diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index eecc896c7..1389865f9 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -60,11 +60,14 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: firstCollision = worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects, completedTimeSteps); if (firstCollision.has_value()) { commandCollision.collisionData = firstCollision; - if ((firstCollision.value().collisionType == BB::CollisionType::BALL || firstCollision.value().collisionType == BB::CollisionType::BALLPLACEMENT) && - firstCollision.value().collisionTime <= 0.11) { - targetPosition = - currentPosition + (currentPosition - world->getWorld()->getBall()->get()->position).stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2); - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); + + if ((firstCollision.value().collisionType == BB::CollisionType::BALL) && firstCollision.value().collisionTime <= 0.11) { + targetPosition = handleBallCollision(world, field, currentPosition, avoidObjects); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, 1.0, ai::Constants::MAX_ACC_UPPER()); + } + if ((firstCollision.value().collisionType == BB::CollisionType::BALLPLACEMENT) && firstCollision.value().collisionTime <= 0.11) { + targetPosition = handleBallPlacementCollision(world, field, currentPosition, avoidObjects); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, 1.0, ai::Constants::MAX_ACC_UPPER()); } } } @@ -122,6 +125,57 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: return commandCollision; } +rtt::Vector2 PositionControl::handleBallCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects) { + auto ballPos = world->getWorld()->getBall()->get()->position; + auto direction = currentPosition - ballPos; + Vector2 targetPosition = currentPosition + direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2); + if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) { + return targetPosition; + } + int rotationStepDegrees = 10; + int maxRotationDegrees = 90; + for (int i = rotationStepDegrees; i <= maxRotationDegrees; i += rotationStepDegrees) { + for (int sign : {1, -1}) { + double rotation = sign * i * M_PI / 180; + Vector2 rotatedDirection = direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2).rotate(rotation); + Vector2 potentialTargetPosition = currentPosition + rotatedDirection; + if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) { + return potentialTargetPosition; + } + } + } + return currentPosition + direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2).rotate(M_PI / 2); +} + +rtt::Vector2 PositionControl::handleBallPlacementCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects) { + auto placementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); + auto ballPos = world->getWorld()->getBall()->get()->position; + auto direction = (placementPos - ballPos).stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2); + double distance = (placementPos - ballPos).length(); + if (distance > 0.10) { // distance is more than 10cm + direction = direction.rotate((currentPosition - ballPos).cross(placementPos - ballPos) < 0 ? M_PI / 2 : -M_PI / 2); + } else { // distance is less than or equal to 10cm + direction = (currentPosition - ballPos).stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2); + } + Vector2 targetPosition = currentPosition + direction; + if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) { + return targetPosition; + } + int rotationStepDegrees = 10; + int maxRotationDegrees = 90; + for (int i = rotationStepDegrees; i <= maxRotationDegrees; i += rotationStepDegrees) { + for (int sign : {1, -1}) { + double rotation = sign * i * M_PI / 180; + Vector2 rotatedDirection = direction.rotate(rotation); + Vector2 potentialTargetPosition = currentPosition + rotatedDirection; + if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) { + return potentialTargetPosition; + } + } + } + return targetPosition; +} + double PositionControl::calculateScore(const rtt::world::World *world, const rtt::Field &field, std::optional &firstCollision, Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects, double startTime) { double totalTime = trajectoryAroundCollision.getTotalTime(); From 7d43550921cfce5c07c1a445cdbaaa5c1ac3099d Mon Sep 17 00:00:00 2001 From: Luuk Date: Tue, 13 Feb 2024 19:36:06 +0100 Subject: [PATCH 10/22] Add drawing for margins and the location of the ball according to AI in the interface --- roboteam_ai/include/roboteam_ai/stp/Play.hpp | 6 + roboteam_ai/src/stp/Play.cpp | 168 +++++++++++++++++++ roboteam_ai/src/world/Ball.cpp | 11 ++ 3 files changed, 185 insertions(+) diff --git a/roboteam_ai/include/roboteam_ai/stp/Play.hpp b/roboteam_ai/include/roboteam_ai/stp/Play.hpp index f3038c13f..9a0cf08c1 100644 --- a/roboteam_ai/include/roboteam_ai/stp/Play.hpp +++ b/roboteam_ai/include/roboteam_ai/stp/Play.hpp @@ -156,6 +156,12 @@ class Play { */ void reassignRobots() noexcept; + /** + * @brief Draws the margins for the defence area, ball and cardRobot. + * This function will draw margins in the interfacce + */ + void DrawMargins() noexcept; + size_t previousRobotNum{}; /**< The previous amount of robots. This is used to check if we need to re-deal (if a robot disappears for example) */ int previousKeeperId = -1; /**< The previous keeperId. This is used to check if we need to re-deal (if keeper id was changed from UI or GameController) */ diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index 71ea5d044..925e807f5 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -5,6 +5,7 @@ #include "stp/Play.hpp" #include "control/ControlUtils.h" +#include "gui/Out.h" namespace rtt::ai::stp { @@ -76,6 +77,7 @@ void Play::update() noexcept { roleStatuses[role.get()] = roleStatus; } } + DrawMargins(); } void Play::reassignRobots() noexcept { @@ -181,4 +183,170 @@ uint8_t Play::getLastScore() const { return lastScore.value_or(0); } bool Play::shouldEndPlay() noexcept { return false; } +void Play::DrawMargins() noexcept { + std::array arr = {world->getWorld()->getBall()->get()->position}; + std::array arrRightDefense = {field.rightDefenseArea.topRight() + Vector2(0.0, 0.2), field.rightDefenseArea.topLeft() + Vector2(-0.2, 0.2), + field.rightDefenseArea.bottomLeft() + Vector2(-0.2, -0.2), field.rightDefenseArea.bottomRight() + Vector2(0.0, -0.2)}; + std::array arrLeftDefense = {field.leftDefenseArea.topLeft() + Vector2(0.0, 0.2), field.leftDefenseArea.topRight() + Vector2(0.2, 0.2), + field.leftDefenseArea.bottomRight() + Vector2(0.2, -0.2), field.leftDefenseArea.bottomLeft() + Vector2(0.0, -0.2)}; + std::span spanRightDefense(arrRightDefense); + std::span span(arr); + std::span spanLeftDefense(arrLeftDefense); + RuleSetName ruleSetTitle = GameStateManager::getCurrentGameState().getRuleSet().getTitle(); + RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); + if (ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM_STOP || + currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM) { + rtt::ai::gui::Out::draw( + { + .label = "Left defense area to avoid", + .color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW, + .method = proto::Drawing::LINES_CONNECTED, + .size = 8, + .thickness = 8, + }, + spanLeftDefense); + + rtt::ai::gui::Out::draw( + { + .label = "Right defense area to avoid", + .color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE, + .method = proto::Drawing::LINES_CONNECTED, + .size = 8, + .thickness = 8, + }, + spanRightDefense); + + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) { + rtt::ai::gui::Out::draw( + { + .label = "Ball area to avoid", + .color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE, + .method = proto::Drawing::CIRCLES, + .size = 53, + .thickness = 3, + }, + span); + } else if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || + currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { + rtt::ai::gui::Out::draw( + { + .label = "Ball area to avoid", + .color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW, + .method = proto::Drawing::CIRCLES, + .size = 53, + .thickness = 3, + }, + span); + } else { + rtt::ai::gui::Out::draw( + { + .label = "Ball area to avoid", + .color = proto::Drawing::GREEN, + .method = proto::Drawing::CIRCLES, + .size = 53, + .thickness = 3, + }, + span); + } + } else { + rtt::ai::gui::Out::draw( + { + .label = "Ball area to avoid", + .color = proto::Drawing::BLUE, + .method = proto::Drawing::CIRCLES, + .size = 0, + .thickness = 0, + }, + span); + + rtt::ai::gui::Out::draw( + { + .label = "Left defense area to avoid", + .color = proto::Drawing::BLUE, + .method = proto::Drawing::CIRCLES, + .size = 0, + .thickness = 0, + }, + spanLeftDefense); + + rtt::ai::gui::Out::draw( + { + .label = "Right defense area to avoid", + .color = proto::Drawing::YELLOW, + .method = proto::Drawing::CIRCLES, + .size = 0, + .thickness = 0, + }, + spanRightDefense); + } + std::array arrToBallPlacement = {world->getWorld()->getBall()->get()->position, rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; + std::array arrBallPlacementLocation = {rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; + std::span spanToBallPlacement(arrToBallPlacement); + std::span spanBallPlacementLocation(arrBallPlacementLocation); + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { + rtt::ai::gui::Out::draw( + { + .label = "Placement location", + .color = proto::Drawing::BLACK, + .method = proto::Drawing::CROSSES, + .size = 10, + .thickness = 3, + }, + spanBallPlacementLocation); + + rtt::ai::gui::Out::draw( + { + .label = "Path to placement location", + .color = proto::Drawing::BLACK, + .method = proto::Drawing::LINES_CONNECTED, + .size = 8, + .thickness = 8, + }, + spanToBallPlacement); + } else { + rtt::ai::gui::Out::draw( + { + .label = "Placement location", + .color = proto::Drawing::BLACK, + .method = proto::Drawing::CROSSES, + .size = 0, + .thickness = 0, + }, + spanBallPlacementLocation); + + rtt::ai::gui::Out::draw( + { + .label = "Path to placement location", + .color = proto::Drawing::BLACK, + .method = proto::Drawing::CROSSES, + .size = 0, + .thickness = 0, + }, + spanToBallPlacement); + } + std::array arrCardId = {rtt::Vector2(0.0, -field.playArea.height() / 2)}; + std::span spanCardId(arrCardId); + if (GameStateManager::getCurrentGameState().cardId != -1) { + rtt::ai::gui::Out::draw( + { + .label = "CardID", + .color = proto::Drawing::BLACK, + .method = proto::Drawing::CIRCLES, + .size = 15, + .thickness = 10, + }, + spanCardId); + } else { + rtt::ai::gui::Out::draw( + { + .label = "CardID", + .color = proto::Drawing::BLACK, + .method = proto::Drawing::CIRCLES, + .size = 0, + .thickness = 0, + }, + spanCardId); + } +} + } // namespace rtt::ai::stp diff --git a/roboteam_ai/src/world/Ball.cpp b/roboteam_ai/src/world/Ball.cpp index 967c4c7bd..c1d752d0e 100644 --- a/roboteam_ai/src/world/Ball.cpp +++ b/roboteam_ai/src/world/Ball.cpp @@ -30,6 +30,17 @@ Ball::Ball(const proto::WorldBall& copy, const World* data) : position{copy.pos( } updateBallAtRobotPosition(data); updateExpectedBallEndPosition(data); + if (data->getWorld() and data->getWorld()->getBall()) { + auto ballPosition = data->getWorld()->getBall().value()->position; + std::array arr = {ballPosition}; + std::span span(arr); + rtt::ai::gui::Out::draw( + { + .label = "Ball according to the AI", .color = proto::Drawing::CYAN, .method = proto::Drawing::CIRCLES, .size = 4, + // .thickness = 4, + }, + span); + } } void Ball::initBallAtExpectedPosition(const world::World* data) noexcept { From 5b0cfebaf4fbea3ad852f6a0b5a75277f93cc99a Mon Sep 17 00:00:00 2001 From: Luuk Date: Thu, 15 Feb 2024 14:51:21 +0100 Subject: [PATCH 11/22] Add drawing for margins and the location of the ball according to AI in the interface --- roboteam_ai/src/stp/Play.cpp | 160 +++++++++++------------------------ 1 file changed, 48 insertions(+), 112 deletions(-) diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index 925e807f5..b05fa56da 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -189,11 +189,34 @@ void Play::DrawMargins() noexcept { field.rightDefenseArea.bottomLeft() + Vector2(-0.2, -0.2), field.rightDefenseArea.bottomRight() + Vector2(0.0, -0.2)}; std::array arrLeftDefense = {field.leftDefenseArea.topLeft() + Vector2(0.0, 0.2), field.leftDefenseArea.topRight() + Vector2(0.2, 0.2), field.leftDefenseArea.bottomRight() + Vector2(0.2, -0.2), field.leftDefenseArea.bottomLeft() + Vector2(0.0, -0.2)}; - std::span spanRightDefense(arrRightDefense); + std::array arrCardId = {rtt::Vector2(0.0, -field.playArea.height() / 2)}; + std::array arrToBallPlacement = {world->getWorld()->getBall()->get()->position, rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; + std::array arrBallPlacementLocation = {rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; std::span span(arr); + std::span spanRightDefense(arrRightDefense); std::span spanLeftDefense(arrLeftDefense); + std::span spanCardId(arrCardId); + std::span spanToBallPlacement(arrToBallPlacement); + std::span spanBallPlacementLocation(arrBallPlacementLocation); + RuleSetName ruleSetTitle = GameStateManager::getCurrentGameState().getRuleSet().getTitle(); RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); + auto color = proto::Drawing::BLUE; + + //Resetting all the drawn figures + for (const auto &label : {"Left defense area to avoid", "Right defense area to avoid", "Ball area to avoid", "Placement location", "Path to placement location", "CardID"}) { + rtt::ai::gui::Out::draw( + { + .label = label, + .color = proto::Drawing::YELLOW, + .method = proto::Drawing::CIRCLES, + .size = 0, + .thickness = 0, + }, + span); + } + + //Drawing all figures regarding states robots have to avoid certain area's (stop, ball placement, free kick, kick off) if (ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM_STOP || currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM) { rtt::ai::gui::Out::draw( @@ -216,116 +239,39 @@ void Play::DrawMargins() noexcept { }, spanRightDefense); - if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) { - rtt::ai::gui::Out::draw( - { - .label = "Ball area to avoid", - .color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE, - .method = proto::Drawing::CIRCLES, - .size = 53, - .thickness = 3, - }, - span); - } else if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || - currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { - rtt::ai::gui::Out::draw( - { - .label = "Ball area to avoid", - .color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW, - .method = proto::Drawing::CIRCLES, - .size = 53, - .thickness = 3, - }, - span); - } else { - rtt::ai::gui::Out::draw( - { - .label = "Ball area to avoid", - .color = proto::Drawing::GREEN, - .method = proto::Drawing::CIRCLES, - .size = 53, - .thickness = 3, - }, - span); - } - } else { + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE; + else if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || + currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW; + else color = proto::Drawing::GREEN; + rtt::ai::gui::Out::draw( { .label = "Ball area to avoid", - .color = proto::Drawing::BLUE, + .color = color, .method = proto::Drawing::CIRCLES, - .size = 0, - .thickness = 0, + .size = 53, + .thickness = 3, }, span); + } - rtt::ai::gui::Out::draw( - { - .label = "Left defense area to avoid", - .color = proto::Drawing::BLUE, - .method = proto::Drawing::CIRCLES, - .size = 0, - .thickness = 0, - }, - spanLeftDefense); - - rtt::ai::gui::Out::draw( - { - .label = "Right defense area to avoid", - .color = proto::Drawing::YELLOW, - .method = proto::Drawing::CIRCLES, - .size = 0, - .thickness = 0, - }, - spanRightDefense); - } - std::array arrToBallPlacement = {world->getWorld()->getBall()->get()->position, rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; - std::array arrBallPlacementLocation = {rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; - std::span spanToBallPlacement(arrToBallPlacement); - std::span spanBallPlacementLocation(arrBallPlacementLocation); + //Drawing all figures regarding ball placement location and the path towards it if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { - rtt::ai::gui::Out::draw( - { - .label = "Placement location", - .color = proto::Drawing::BLACK, - .method = proto::Drawing::CROSSES, - .size = 10, - .thickness = 3, - }, - spanBallPlacementLocation); - - rtt::ai::gui::Out::draw( - { - .label = "Path to placement location", - .color = proto::Drawing::BLACK, - .method = proto::Drawing::LINES_CONNECTED, - .size = 8, - .thickness = 8, - }, - spanToBallPlacement); - } else { - rtt::ai::gui::Out::draw( - { - .label = "Placement location", - .color = proto::Drawing::BLACK, - .method = proto::Drawing::CROSSES, - .size = 0, - .thickness = 0, - }, - spanBallPlacementLocation); - - rtt::ai::gui::Out::draw( - { - .label = "Path to placement location", - .color = proto::Drawing::BLACK, - .method = proto::Drawing::CROSSES, - .size = 0, - .thickness = 0, - }, - spanToBallPlacement); + for (auto method : {proto::Drawing::CROSSES, proto::Drawing::LINES_CONNECTED}) { + RTT_INFO(method) + rtt::ai::gui::Out::draw( + { + .label = method == proto::Drawing::CROSSES ? "Placement location" : "Path to placement location ", + .color = proto::Drawing::BLACK, + .method = method, + .size = method == proto::Drawing::CROSSES ? 10 : 8, + .thickness = method == proto::Drawing::CROSSES ? 5 : 8, + }, + spanToBallPlacement); + } } - std::array arrCardId = {rtt::Vector2(0.0, -field.playArea.height() / 2)}; - std::span spanCardId(arrCardId); + + if (GameStateManager::getCurrentGameState().cardId != -1) { rtt::ai::gui::Out::draw( { @@ -336,16 +282,6 @@ void Play::DrawMargins() noexcept { .thickness = 10, }, spanCardId); - } else { - rtt::ai::gui::Out::draw( - { - .label = "CardID", - .color = proto::Drawing::BLACK, - .method = proto::Drawing::CIRCLES, - .size = 0, - .thickness = 0, - }, - spanCardId); } } From d19322d62cb27fd76d5cd7f21c1c376f80d9b6c5 Mon Sep 17 00:00:00 2001 From: Jorn Date: Thu, 15 Feb 2024 15:49:02 +0100 Subject: [PATCH 12/22] Update ball position guesses when vision has troubles finding the correct spot --- .../stp/constants/ControlConstants.h | 8 ++- roboteam_ai/src/world/Ball.cpp | 67 +++++++++---------- 2 files changed, 36 insertions(+), 39 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index b40b8b145..05fc84b9f 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -19,8 +19,8 @@ constexpr double MAX_KICK_ATTEMPTS = 25; /**< Maximum allowed kicking attempts * constexpr double MAX_CHIP_ATTEMPTS = 25; /**< Maximum allowed chipping attempts */ /// Robot physical constants -constexpr double ROBOT_RADIUS = 0.088; /**< Radius of a robot */ -constexpr double CENTER_TO_FRONT = 0.05; /**< Distance from center of the robot to the front of the robot */ +constexpr double ROBOT_RADIUS = 0.088; /**< Radius of a robot */ +constexpr double CENTER_TO_FRONT = 0.069; /**< Distance from center of the robot to the front of the robot */ /// Dribbler constants // The distance from robot to ball at which the dribbler should turn on @@ -81,6 +81,10 @@ constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARG 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 */ +/// Friction constants +constexpr static float SIMULATION_FRICTION = 0.69; /**< * The expected movement friction of the ball during simulation */ +constexpr static float REAL_FRICTION = 0.5; /**< The expected movement friction of the ball on the field */ + } // namespace rtt::ai::stp::control_constants #endif // RTT_CONTROLCONSTANTS_H diff --git a/roboteam_ai/src/world/Ball.cpp b/roboteam_ai/src/world/Ball.cpp index c1d752d0e..3a4be119a 100644 --- a/roboteam_ai/src/world/Ball.cpp +++ b/roboteam_ai/src/world/Ball.cpp @@ -11,41 +11,28 @@ namespace rtt::world::ball { -/** - * The movement friction during simulation and real life are different, because the simulation does not model - * everything. So the movement friction has to be adjusted to compensate for this difference. - * - * The expected movement friction of the ball during simulation - */ -constexpr static float SIMULATION_FRICTION = 0.69; - -/** - * The expected movement friction of the ball on the field - */ -constexpr static float REAL_FRICTION = 0.5; - Ball::Ball(const proto::WorldBall& copy, const World* data) : position{copy.pos().x(), copy.pos().y()}, velocity{copy.vel().x(), copy.vel().y()}, visible{copy.visible()} { if (!visible || position == Vector2()) { initBallAtExpectedPosition(data); } updateBallAtRobotPosition(data); updateExpectedBallEndPosition(data); - if (data->getWorld() and data->getWorld()->getBall()) { - auto ballPosition = data->getWorld()->getBall().value()->position; - std::array arr = {ballPosition}; - std::span span(arr); + + if (position != Vector2()) { + std::array point = {position}; rtt::ai::gui::Out::draw( { - .label = "Ball according to the AI", .color = proto::Drawing::CYAN, .method = proto::Drawing::CIRCLES, .size = 4, - // .thickness = 4, + .label = "position_ball_AI", + .color = proto::Drawing::CYAN, + .method = proto::Drawing::CIRCLES, + .size = 4, }, - span); + point); } } void Ball::initBallAtExpectedPosition(const world::World* data) noexcept { - std::optional previousWorld = data->getHistoryWorld(1); - + auto previousWorld = data->getHistoryWorld(1); if (!previousWorld || !previousWorld->getBall()) { return; } @@ -53,21 +40,18 @@ void Ball::initBallAtExpectedPosition(const world::World* data) noexcept { } void Ball::updateExpectedBallEndPosition(const world::World* data) noexcept { - std::optional previousWorld = data->getHistoryWorld(1); - + auto previousWorld = data->getHistoryWorld(1); if (!previousWorld || !previousWorld->getBall()) { return; } auto ball = previousWorld->getBall().value(); - - double ballVelSquared = ball->velocity.length2(); - const double frictionCoefficient = GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? SIMULATION_FRICTION : REAL_FRICTION; - + const double ballVelSquared = ball->velocity.length2(); + const double frictionCoefficient = + GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::stp::control_constants::SIMULATION_FRICTION : ai::stp::control_constants::REAL_FRICTION; expectedEndPosition = ball->position + ball->velocity.stretchToLength(ballVelSquared / frictionCoefficient); - std::array arr = {expectedEndPosition, ball->position}; - std::span span(arr); - // maybe change to a cross at the end instead of a line? + + std::array ballPoints = {expectedEndPosition, ball->position}; rtt::ai::gui::Out::draw( { .label = "expected_end_position_ball", @@ -75,23 +59,32 @@ void Ball::updateExpectedBallEndPosition(const world::World* data) noexcept { .method = proto::Drawing::LINES_CONNECTED, .thickness = 3, }, - span); + ballPoints); } void Ball::updateBallAtRobotPosition(const world::World* data) noexcept { - std::optional world = data->getWorld(); - if (!world.has_value()) return; + auto world = data->getWorld(); + if (!world.has_value()) { + return; + } - std::optional robotWithBall = world->whichRobotHasBall(); + auto robotWithBall = world->whichRobotHasBall(); if (!robotWithBall.has_value()) { return; } - if ((robotWithBall->get()->getVel() - velocity).length() > 0.1) { + + if ((robotWithBall->get()->getVel() - velocity).length() > 1.6) { return; } - // Place the ball where we would expect it to be given that this robot has the ball + + auto robotClostestToPoint = world->getRobotClosestToPoint(position, Team::both); + if (robotClostestToPoint.has_value() && (robotClostestToPoint->get()->getPos() - position).length() > 0.2) { + return; + } + double distanceInFrontOfRobot = ai::stp::control_constants::CENTER_TO_FRONT + ai::Constants::BALL_RADIUS(); position = robotWithBall->get()->getPos() + robotWithBall->get()->getAngle().toVector2(distanceInFrontOfRobot); + velocity = robotWithBall->get()->getVel(); } } // namespace rtt::world::ball From 4efca1185e25bd41cdfd5a6ad3c5358a2a03d282 Mon Sep 17 00:00:00 2001 From: Jorn Date: Thu, 15 Feb 2024 17:27:52 +0100 Subject: [PATCH 13/22] Clean up PenaltyKeeper update function --- roboteam_ai/src/stp/roles/PenaltyKeeper.cpp | 37 +++------------------ 1 file changed, 4 insertions(+), 33 deletions(-) diff --git a/roboteam_ai/src/stp/roles/PenaltyKeeper.cpp b/roboteam_ai/src/stp/roles/PenaltyKeeper.cpp index 986100f53..0bb4c0fd0 100644 --- a/roboteam_ai/src/stp/roles/PenaltyKeeper.cpp +++ b/roboteam_ai/src/stp/roles/PenaltyKeeper.cpp @@ -33,44 +33,15 @@ PenaltyKeeper::PenaltyKeeper(std::string name) : Keeper(std::move(name)) { Status PenaltyKeeper::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") + RTT_WARNING("Required information missing in the tactic info for ", roleName) return Status::Failure; } // Stop Formation tactic when ball is moving, start blocking, getting the ball and pass (normal keeper behavior) - if (robotTactics.current_num() == 0 && info.getBall().value()->velocity.length() > control_constants::BALL_STILL_VEL) forceNextTactic(); - - currentRobot = info.getRobot(); - // Update the current tactic with the new tacticInfo - auto status = robotTactics.update(info); - - // Success if the tactic returned success and if all tactics are done - if (status == Status::Success && robotTactics.finished()) { - RTT_INFO("ROLE SUCCESSFUL for ", info.getRobot()->get()->getId()) - return Status::Success; - } - - // Reset the tactic state machine if a tactic failed and the state machine is not yet finished - if ((status == Status::Failure && !robotTactics.finished())) { - RTT_INFO("State Machine reset for current role for ID = ", info.getRobot()->get()->getId()) - // Reset all the Skills state machines - for (auto& tactic : robotTactics) { - tactic->reset(); - } - // Reset Tactics state machine - robotTactics.reset(); - } - - // Success if waiting and tactics are finished - // Waiting if waiting but not finished - if (status == Status::Waiting) { - if (robotTactics.finished()) { - return Status::Success; - } - return Status::Waiting; + if (robotTactics.current_num() == 0 && info.getBall().value()->velocity.length() > control_constants::BALL_STILL_VEL) { + forceNextTactic(); } - // Return running by default - return Status::Running; + return Role::update(info); } } // namespace rtt::ai::stp::role From 1b5e96ae93c1396b0149bf404909498cd6128c75 Mon Sep 17 00:00:00 2001 From: Jorn Date: Thu, 15 Feb 2024 17:30:46 +0100 Subject: [PATCH 14/22] Update PenaltyTaker behaviour to not keep the ball for more than 1m. Still needs tweaking with real robots. Note that GetBall when the ball is moving needs to be improved before this will work good --- .../stp/roles/active/PenaltyTaker.h | 9 +++++- .../stp/plays/referee_specific/PenaltyUs.cpp | 9 ++++-- .../src/stp/roles/active/PenaltyTaker.cpp | 29 ++++++++++++++++++- 3 files changed, 42 insertions(+), 5 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/roles/active/PenaltyTaker.h b/roboteam_ai/include/roboteam_ai/stp/roles/active/PenaltyTaker.h index bab94607f..bfe581e5a 100644 --- a/roboteam_ai/include/roboteam_ai/stp/roles/active/PenaltyTaker.h +++ b/roboteam_ai/include/roboteam_ai/stp/roles/active/PenaltyTaker.h @@ -14,7 +14,14 @@ class PenaltyTaker : public Role { * @param name The name of the role */ PenaltyTaker(std::string name); + + /** + * @brief Method that updates the role + * @param info The information to update the role with + * @return Status of the update + */ + [[nodiscard]] Status update(StpInfo const& info) noexcept override; }; } // namespace rtt::ai::stp::role -#endif // RTT_PENALTYTAKER_H +#endif // RTT_PENALTYTAKER_H \ No newline at end of file diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp index a319a79a1..48e3ac93c 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp @@ -60,7 +60,6 @@ Dealer::FlagMap PenaltyUs::decideRoleFlags() const noexcept { } void PenaltyUs::calculateInfoForRoles() noexcept { - // Function where are roles get their information, make sure not to compute roles twice. PositionComputations::calculateInfoForKeeper(stpInfos, field, world); auto positionTarget = PositionComputations::getPosition(std::nullopt, field.middleRightGrid, gen::GoalShot, field, world); @@ -68,8 +67,12 @@ void PenaltyUs::calculateInfoForRoles() noexcept { auto goalTarget = computations::GoalComputations::calculateGoalTarget(world, field); stpInfos["kicker"].setPositionToShootAt(goalTarget); stpInfos["kicker"].setShotType(ShotType::MAX); - if (stpInfos["kicker"].getRobot().has_value()) - stpInfos["kicker"].setMaxRobotVelocity(std::max((stpInfos["kicker"].getRobot()->get()->getPos() - positionTarget.position).length(), 0.5)); + if (stpInfos["kicker"].getRobot().has_value() && stpInfos["kicker"].getRobot()->get()->hasBall()) { + stpInfos["kicker"].setMaxRobotVelocity(1.0); + if ((stpInfos["kicker"].getRobot()->get()->getPos() - positionTarget.position).length() < 0.5) { + stpInfos["kicker"].setMaxRobotVelocity(0.7); + } + } } } // namespace rtt::ai::stp::play diff --git a/roboteam_ai/src/stp/roles/active/PenaltyTaker.cpp b/roboteam_ai/src/stp/roles/active/PenaltyTaker.cpp index 031b13588..5da5825c6 100644 --- a/roboteam_ai/src/stp/roles/active/PenaltyTaker.cpp +++ b/roboteam_ai/src/stp/roles/active/PenaltyTaker.cpp @@ -10,4 +10,31 @@ PenaltyTaker::PenaltyTaker(std::string name) : Role(std::move(name)) { // create state machine and initializes the first state robotTactics = collections::state_machine{tactic::GetBall(), tactic::DriveWithBall(), tactic::OrbitKick()}; } -} // namespace rtt::ai::stp::role + +[[nodiscard]] Status PenaltyTaker::update(StpInfo const& info) noexcept { + StpInfo infoCopy = info; + static double distanceDriven = 0.0; + static Vector2 lastPosition; + + if (info.getRobot().has_value()) { + auto currentPos = info.getRobot()->get()->getPos(); + auto currentVel = info.getRobot()->get()->getVel(); + distanceDriven += (currentPos - lastPosition).length(); + lastPosition = currentPos; + if (distanceDriven >= 0.5 && (info.getPositionToMoveTo().value() - currentPos).length() > 0.4) { + // set position to move to the current position + infoCopy.setPositionToMoveTo(currentPos - Vector2(0.1, 0)); + if (currentVel.x < 0.1) { + infoCopy.setDribblerSpeed(0); + } + } + // if we don't have ball, reset the tactic + if (!info.getRobot()->get()->hasBall()) { + distanceDriven = 0.0; + reset(); + } + } + + return Role::update(infoCopy); +} +} // namespace rtt::ai::stp::role \ No newline at end of file From 8ee4b2b13fe578d8ac68e3031b55d77551ca4277 Mon Sep 17 00:00:00 2001 From: Luuk Date: Fri, 16 Feb 2024 10:40:59 +0100 Subject: [PATCH 15/22] Add drawing for margins and the location of the ball according to AI in the interface --- roboteam_ai/src/stp/Play.cpp | 63 +++++++++++++++++------------------- 1 file changed, 29 insertions(+), 34 deletions(-) diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index b05fa56da..cdd0f59ae 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -184,26 +184,19 @@ uint8_t Play::getLastScore() const { return lastScore.value_or(0); } bool Play::shouldEndPlay() noexcept { return false; } void Play::DrawMargins() noexcept { - std::array arr = {world->getWorld()->getBall()->get()->position}; - std::array arrRightDefense = {field.rightDefenseArea.topRight() + Vector2(0.0, 0.2), field.rightDefenseArea.topLeft() + Vector2(-0.2, 0.2), - field.rightDefenseArea.bottomLeft() + Vector2(-0.2, -0.2), field.rightDefenseArea.bottomRight() + Vector2(0.0, -0.2)}; - std::array arrLeftDefense = {field.leftDefenseArea.topLeft() + Vector2(0.0, 0.2), field.leftDefenseArea.topRight() + Vector2(0.2, 0.2), - field.leftDefenseArea.bottomRight() + Vector2(0.2, -0.2), field.leftDefenseArea.bottomLeft() + Vector2(0.0, -0.2)}; - std::array arrCardId = {rtt::Vector2(0.0, -field.playArea.height() / 2)}; - std::array arrToBallPlacement = {world->getWorld()->getBall()->get()->position, rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; - std::array arrBallPlacementLocation = {rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; - std::span span(arr); - std::span spanRightDefense(arrRightDefense); - std::span spanLeftDefense(arrLeftDefense); - std::span spanCardId(arrCardId); - std::span spanToBallPlacement(arrToBallPlacement); - std::span spanBallPlacementLocation(arrBallPlacementLocation); + std::array ball = {world->getWorld()->getBall()->get()->position}; + std::array rightDefenseAreaMargin = {field.rightDefenseArea.topRight() + Vector2(0.0, 0.2), field.rightDefenseArea.topLeft() + Vector2(-0.2, 0.2), + field.rightDefenseArea.bottomLeft() + Vector2(-0.2, -0.2), field.rightDefenseArea.bottomRight() + Vector2(0.0, -0.2)}; + std::array leftDefenseAreaMargin = {field.leftDefenseArea.topLeft() + Vector2(0.0, 0.2), field.leftDefenseArea.topRight() + Vector2(0.2, 0.2), + field.leftDefenseArea.bottomRight() + Vector2(0.2, -0.2), field.leftDefenseArea.bottomLeft() + Vector2(0.0, -0.2)}; + std::array cardId = {rtt::Vector2(0.0, -field.playArea.height() / 2)}; + std::array pathToBallPlacement = {world->getWorld()->getBall()->get()->position, rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; RuleSetName ruleSetTitle = GameStateManager::getCurrentGameState().getRuleSet().getTitle(); RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); auto color = proto::Drawing::BLUE; - //Resetting all the drawn figures + // Resetting all the drawn figures for (const auto &label : {"Left defense area to avoid", "Right defense area to avoid", "Ball area to avoid", "Placement location", "Path to placement location", "CardID"}) { rtt::ai::gui::Out::draw( { @@ -213,12 +206,13 @@ void Play::DrawMargins() noexcept { .size = 0, .thickness = 0, }, - span); + ball); } - //Drawing all figures regarding states robots have to avoid certain area's (stop, ball placement, free kick, kick off) - if (ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM_STOP || - currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM) { + // Drawing all figures regarding states robots have to avoid certain area's (stop, ball placement, free kick, kick off) + if ((ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM_STOP || + currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM) && + currentGameState != RefCommand::BALL_PLACEMENT_THEM && currentGameState != RefCommand::BALL_PLACEMENT_US && currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { rtt::ai::gui::Out::draw( { .label = "Left defense area to avoid", @@ -227,8 +221,7 @@ void Play::DrawMargins() noexcept { .size = 8, .thickness = 8, }, - spanLeftDefense); - + leftDefenseAreaMargin); rtt::ai::gui::Out::draw( { .label = "Right defense area to avoid", @@ -237,13 +230,16 @@ void Play::DrawMargins() noexcept { .size = 8, .thickness = 8, }, - spanRightDefense); + rightDefenseAreaMargin); + + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) + color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE; + else if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || currentGameState == RefCommand::DIRECT_FREE_US || + currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) + color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW; + else + color = proto::Drawing::GREEN; - if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE; - else if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || - currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW; - else color = proto::Drawing::GREEN; - rtt::ai::gui::Out::draw( { .label = "Ball area to avoid", @@ -252,10 +248,10 @@ void Play::DrawMargins() noexcept { .size = 53, .thickness = 3, }, - span); - } + ball); + } - //Drawing all figures regarding ball placement location and the path towards it + // Drawing all figures regarding ball placement location and the path towards it if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { for (auto method : {proto::Drawing::CROSSES, proto::Drawing::LINES_CONNECTED}) { RTT_INFO(method) @@ -267,11 +263,10 @@ void Play::DrawMargins() noexcept { .size = method == proto::Drawing::CROSSES ? 10 : 8, .thickness = method == proto::Drawing::CROSSES ? 5 : 8, }, - spanToBallPlacement); + pathToBallPlacement); } } - - + if (GameStateManager::getCurrentGameState().cardId != -1) { rtt::ai::gui::Out::draw( { @@ -281,7 +276,7 @@ void Play::DrawMargins() noexcept { .size = 15, .thickness = 10, }, - spanCardId); + cardId); } } From 028dc0c0511919f4dadd57b7561fd68a0889fcf3 Mon Sep 17 00:00:00 2001 From: Luuk Date: Fri, 16 Feb 2024 11:19:32 +0100 Subject: [PATCH 16/22] Add drawing for margins and the location of the ball according to AI in the interface --- roboteam_ai/src/stp/Play.cpp | 56 ++++++++++++++---------------------- 1 file changed, 22 insertions(+), 34 deletions(-) diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index cdd0f59ae..7d93aff43 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -196,41 +196,29 @@ void Play::DrawMargins() noexcept { RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); auto color = proto::Drawing::BLUE; - // Resetting all the drawn figures - for (const auto &label : {"Left defense area to avoid", "Right defense area to avoid", "Ball area to avoid", "Placement location", "Path to placement location", "CardID"}) { - rtt::ai::gui::Out::draw( - { - .label = label, - .color = proto::Drawing::YELLOW, - .method = proto::Drawing::CIRCLES, - .size = 0, - .thickness = 0, - }, - ball); - } - // Drawing all figures regarding states robots have to avoid certain area's (stop, ball placement, free kick, kick off) - if ((ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM_STOP || - currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM) && - currentGameState != RefCommand::BALL_PLACEMENT_THEM && currentGameState != RefCommand::BALL_PLACEMENT_US && currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { - rtt::ai::gui::Out::draw( - { - .label = "Left defense area to avoid", - .color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW, - .method = proto::Drawing::LINES_CONNECTED, - .size = 8, - .thickness = 8, - }, - leftDefenseAreaMargin); - rtt::ai::gui::Out::draw( - { - .label = "Right defense area to avoid", - .color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE, - .method = proto::Drawing::LINES_CONNECTED, - .size = 8, - .thickness = 8, - }, - rightDefenseAreaMargin); + if (ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM_STOP || + currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM) { + if (currentGameState != RefCommand::BALL_PLACEMENT_THEM && currentGameState != RefCommand::BALL_PLACEMENT_US && currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { + rtt::ai::gui::Out::draw( + { + .label = "Left defense area to avoid", + .color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW, + .method = proto::Drawing::LINES_CONNECTED, + .size = 8, + .thickness = 8, + }, + leftDefenseAreaMargin); + rtt::ai::gui::Out::draw( + { + .label = "Right defense area to avoid", + .color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE, + .method = proto::Drawing::LINES_CONNECTED, + .size = 8, + .thickness = 8, + }, + rightDefenseAreaMargin); + } if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE; From 136c8a0b7fd355be11e0de4e0df6cba7c14229cd Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 23 Feb 2024 23:11:58 +0100 Subject: [PATCH 17/22] - Let harasser intercept the ball instead of driving after it - Include ball position at time calculation - Improve GetBall when ball is moving away from us --- .../stp/computations/PositionComputations.h | 16 +++++- .../stp/constants/ControlConstants.h | 1 - .../stp/plays/defensive/DefendPass.h | 2 + .../stp/plays/defensive/DefendShot.h | 2 + .../include/roboteam_ai/utilities/Dealer.h | 2 +- .../roboteam_ai/world/FieldComputations.h | 7 +++ roboteam_ai/src/control/ControlModule.cpp | 4 +- .../stp/computations/PositionComputations.cpp | 41 ++++++++++++-- .../src/stp/plays/defensive/DefendPass.cpp | 5 +- .../src/stp/plays/defensive/DefendShot.cpp | 5 +- roboteam_ai/src/stp/skills/GoToPos.cpp | 9 +-- .../src/stp/tactics/active/GetBall.cpp | 55 ++++++++++++++----- roboteam_ai/src/utilities/Constants.cpp | 2 +- roboteam_ai/src/utilities/Dealer.cpp | 16 +----- roboteam_ai/src/world/FieldComputations.cpp | 14 +++++ 15 files changed, 129 insertions(+), 52 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h index ed5e402b6..fdb807394 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h @@ -31,6 +31,11 @@ struct EnemyInfo { int id; }; +struct HarasserInfo { + int harasserId; + double timeToBall; +}; + /** * @brief class with computations about positions */ @@ -96,15 +101,24 @@ class PositionComputations { */ static void calculateInfoForKeeper(std::unordered_map &stpInfos, const Field &field, world::World *world) noexcept; + /** + * @brief Calculates the id of the harasser + * @param world The current world + * @param field The current field + * @return HarasserInfo with the id and the time to the ball + */ + static HarasserInfo calculateHarasserId(rtt::world::World *world, const Field &field) noexcept; + /** * @brief Calculates info for the harasser role * @param stpInfos The current stpInfos * @param roles The current roles * @param field The current field * @param world The current world + * @param timeToBall The time to the ball */ static void calculateInfoForHarasser(std::unordered_map &stpInfos, std::array, stp::control_constants::MAX_ROBOT_COUNT> *roles, - const Field &field, world::World *world) noexcept; + const Field &field, world::World *world, double timeToBall) noexcept; /** * @brief Calculates info for the defenders diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index 05fc84b9f..ded53dcc8 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -53,7 +53,6 @@ constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.04; /**< 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; /**< Distance error for the ball to be considered to have reached the ball placement position*/ -constexpr double DEALER_SPEED_FACTOR = 0.5; /**< Multiplication factor of speed used by the dealer */ 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 diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h index 3bfdd6b41..4b9ea6469 100644 --- a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h +++ b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h @@ -44,6 +44,8 @@ class DefendPass : public Play { * @return The name of the play as string */ const char* getName() const override; + + HarasserInfo harasserInfo; }; } // namespace rtt::ai::stp::play diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h index 3b12a8766..8754a3b51 100644 --- a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h +++ b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h @@ -43,6 +43,8 @@ class DefendShot : public Play { * @brief Gets the play name */ const char* getName() const override; + + HarasserInfo harasserInfo; }; } // namespace rtt::ai::stp::play diff --git a/roboteam_ai/include/roboteam_ai/utilities/Dealer.h b/roboteam_ai/include/roboteam_ai/utilities/Dealer.h index 7004d6f29..5b1fcb947 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/Dealer.h +++ b/roboteam_ai/include/roboteam_ai/utilities/Dealer.h @@ -23,7 +23,7 @@ namespace v = rtt::world::view; /** * @brief Enumerator that defines the name of the dealerFlags */ -enum class DealerFlagTitle { WITH_WORKING_BALL_SENSOR, WITH_WORKING_DRIBBLER, READY_TO_INTERCEPT_GOAL_SHOT, KEEPER, CAN_DETECT_BALL, CAN_KICK_BALL }; +enum class DealerFlagTitle { WITH_WORKING_BALL_SENSOR, WITH_WORKING_DRIBBLER, KEEPER, CAN_DETECT_BALL, CAN_KICK_BALL }; /** * @brief Enumerator that defines the priority of the dealerFlags diff --git a/roboteam_ai/include/roboteam_ai/world/FieldComputations.h b/roboteam_ai/include/roboteam_ai/world/FieldComputations.h index 9dfdceedf..1387d7108 100644 --- a/roboteam_ai/include/roboteam_ai/world/FieldComputations.h +++ b/roboteam_ai/include/roboteam_ai/world/FieldComputations.h @@ -44,6 +44,13 @@ class FieldComputations { */ static bool getBallAvoidance(); + /** + * @brief Get the expected ball position after a certain amount of time + * @param ball The current ball position + * @param time The time in seconds after which the expected ball position should be returned + */ + static Vector2 getBallPositionAtTime(const rtt::world::ball::Ball &ball, double time); + /** * @brief Check whether a given point is a valid position given which parts of the field should be avoided (note that shouldAvoidBall is not taken into consideration) * @param field The field class which is used to determine the boundaries of the field. diff --git a/roboteam_ai/src/control/ControlModule.cpp b/roboteam_ai/src/control/ControlModule.cpp index 2da4f3c36..1b82b8367 100644 --- a/roboteam_ai/src/control/ControlModule.cpp +++ b/roboteam_ai/src/control/ControlModule.cpp @@ -84,9 +84,9 @@ void ControlModule::simulator_angular_control(const std::optional<::rtt::world:: } else { // initialize PID controller for robot // below tuning only works ish for erforce, is completely useless in grsim - double P = 4.0; + double P = 2.5; double I = 0.0; - double D = 0.01; + double D = 0; double max_ang_vel = 5.0; // rad/s double dt = 1. / double(Constants::STP_TICK_RATE()); diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 4cad2ccbc..b0bb1a3c9 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -56,7 +56,7 @@ std::vector PositionComputations::determineWallPositions(const rtt::Fie // Calculate the position of the ball, projected onto the field if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { - ballPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); + ballPos = GameStateManager::getRefereeDesignatedPosition(); } else { ballPos = FieldComputations::projectPointInField(field, world->getWorld().value().getBall()->get()->position); } @@ -255,10 +255,41 @@ void PositionComputations::calculateInfoForKeeper(std::unordered_mapgetWorld()->getRobotClosestToBall(world::them)); } +HarasserInfo PositionComputations::calculateHarasserId(world::World *world, const Field &field) noexcept { + auto maxRobotVelocity = GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); + int harasserId = -1; + int keeperId = GameStateManager::getCurrentGameState().keeperId; + Vector2 newBallPos; + double endTime = 0; + for (double loopTime = 0; loopTime < 5; loopTime += 0.1) { + newBallPos = FieldComputations::getBallPositionAtTime(*(world->getWorld()->getBall()->get()), loopTime); + + if (field.leftDefenseArea.contains(newBallPos)) { + std::vector intersections = + FieldComputations::getDefenseArea(field, true, 0, 0).intersections({newBallPos, world->getWorld()->getBall()->get()->expectedEndPosition}); + if (intersections.size() == 1) newBallPos = intersections.at(0); + } else if (field.rightDefenseArea.contains(newBallPos)) { + std::vector intersections = + FieldComputations::getDefenseArea(field, false, 0, 0).intersections({newBallPos, world->getWorld()->getBall()->get()->expectedEndPosition}); + if (intersections.size() == 1) newBallPos = intersections.at(0); + } + + for (const auto &robot : world->getWorld()->getUs()) { + if (robot->getId() == keeperId) continue; + auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), newBallPos, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); + auto timeToTarget = trajectory.getTotalTime(); + if (timeToTarget < loopTime) { + return {robot->getId(), loopTime}; + } + } + } + return {harasserId, endTime}; +} + void PositionComputations::calculateInfoForHarasser(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> *roles, const Field &field, - world::World *world) noexcept { - rtt::Vector2 ballPos = world->getWorld()->getBall()->get()->position; + std::array, stp::control_constants::MAX_ROBOT_COUNT> *roles, const Field &field, world::World *world, + double timeToBall) noexcept { + rtt::Vector2 ballPos = FieldComputations::getBallPositionAtTime(*(world->getWorld()->getBall()->get()), timeToBall); if (field.leftDefenseArea.contains(ballPos)) { std::vector intersections = FieldComputations::getDefenseArea(field, true, 0, 0).intersections({ballPos, world->getWorld()->getBall()->get()->expectedEndPosition}); @@ -282,7 +313,7 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_mapget()->getPos(); auto targetPos = enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(control_constants::ROBOT_RADIUS * 4); stpInfos["harasser"].setPositionToMoveTo(targetPos); - stpInfos["harasser"].setAngle((ballPos - targetPos).angle()); + stpInfos["harasser"].setAngle((world->getWorld()->getBall()->get()->position - targetPos).angle()); } else { if (enemyClosestToBall->get()->getPos().dist(field.leftGoalArea.rightLine().center()) > stpInfos["harasser"].getRobot()->get()->getPos().dist(field.leftGoalArea.rightLine().center())) { diff --git a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp index 080b45bcb..db35da4ff 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp +++ b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp @@ -58,7 +58,7 @@ Dealer::FlagMap DefendPass::decideRoleFlags() const noexcept { Dealer::DealerFlag keeperFlag(DealerFlagTitle::KEEPER); flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); - flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}}}); + flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}, harasserInfo.harasserId}}); flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); @@ -73,8 +73,9 @@ Dealer::FlagMap DefendPass::decideRoleFlags() const noexcept { } void DefendPass::calculateInfoForRoles() noexcept { + harasserInfo = PositionComputations::calculateHarasserId(world, field); PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world); + PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world, harasserInfo.timeToBall); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); } diff --git a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp b/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp index b53c47db5..87edb44e1 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp +++ b/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp @@ -55,7 +55,7 @@ Dealer::FlagMap DefendShot::decideRoleFlags() const noexcept { Dealer::DealerFlag keeperFlag(DealerFlagTitle::KEEPER); flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); - flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}}}); + flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}, harasserInfo.harasserId}}); flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"waller_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); @@ -70,8 +70,9 @@ Dealer::FlagMap DefendShot::decideRoleFlags() const noexcept { } void DefendShot::calculateInfoForRoles() noexcept { + harasserInfo = PositionComputations::calculateHarasserId(world, field); PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world); + PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world, harasserInfo.timeToBall); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); } diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index a1ab99e29..1e6c079d3 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -34,14 +34,7 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { auto &velocity = commandCollision.robotCommand.velocity; command.velocity = velocity.stretchToLength(std::clamp(velocity.length(), 0.0, info.getMaxRobotVelocity())); - // TODO: Test with control peeps to see what angle works best when driving. - // Driving and turning do not work well together, so we only turn when we are close to the target position. - // This also avoids driving into the defense area when robots are moving just allong the edge of the defense area. - if ((info.getRobot().value()->getPos() - targetPos).length() <= 0.5) { - command.targetAngle = info.getAngle(); - } else { - command.targetAngle = info.getRobot().value()->getAngle(); - } + command.targetAngle = info.getAngle(); // Clamp and set dribbler speed int targetDribblerPercentage = std::clamp(info.getDribblerSpeed(), 0, 100); diff --git a/roboteam_ai/src/stp/tactics/active/GetBall.cpp b/roboteam_ai/src/stp/tactics/active/GetBall.cpp index fc9f4ea55..ae2e03901 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -7,9 +7,12 @@ #include "stp/tactics/active/GetBall.h" +#include + #include "control/ControlUtils.h" #include "stp/skills/GoToPos.h" #include "stp/skills/Rotate.h" +#include "utilities/GameStateManager.hpp" #include "world/FieldComputations.h" namespace rtt::ai::stp::tactic { @@ -21,34 +24,56 @@ std::optional GetBall::calculateInfoForSkill(StpInfo const &info) noexc if (!skillStpInfo.getRobot() || !skillStpInfo.getBall() || !skillStpInfo.getField()) return std::nullopt; Vector2 robotPosition = skillStpInfo.getRobot().value()->getPos(); + Vector2 robotVelocity = skillStpInfo.getRobot().value()->getVel(); + Vector2 interceptionPosition; Vector2 ballPosition = skillStpInfo.getBall().value()->position; - if (skillStpInfo.getField().value().leftDefenseArea.contains(ballPosition)) { - std::vector intersections = - FieldComputations::getDefenseArea(skillStpInfo.getField().value(), true, 0, 0).intersections({ballPosition, skillStpInfo.getBall().value()->expectedEndPosition}); - if (intersections.size() == 1) ballPosition = intersections.at(0); - } else if (skillStpInfo.getField().value().rightDefenseArea.contains(ballPosition)) { - std::vector intersections = - FieldComputations::getDefenseArea(skillStpInfo.getField().value(), false, 0, 0).intersections({ballPosition, skillStpInfo.getBall().value()->expectedEndPosition}); - if (intersections.size() == 1) ballPosition = intersections.at(0); + auto maxRobotVelocity = GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); + if (skillStpInfo.getRobot()->get()->hasBall()) { + maxRobotVelocity = std::clamp(skillStpInfo.getBall().value()->velocity.length() * 0.8, 0.5, maxRobotVelocity); + skillStpInfo.setMaxRobotVelocity(maxRobotVelocity); } - double ballDistance = (ballPosition - robotPosition).length() - control_constants::BALL_RADIUS - control_constants::ROBOT_RADIUS + control_constants::GO_TO_POS_ERROR_MARGIN + - 2 * control_constants::BALL_RADIUS; - - if (skillStpInfo.getRobot()->get()->getAngleDiffToBall() > Constants::HAS_BALL_ANGLE() && ballDistance < control_constants::ROBOT_CLOSE_TO_POINT) { + for (double loopTime = 0; loopTime < 5; loopTime += 0.1) { + interceptionPosition = FieldComputations::getBallPositionAtTime(*(skillStpInfo.getCurrentWorld()->getWorld()->getBall()->get()), loopTime); + if (skillStpInfo.getObjectsToAvoid().shouldAvoidDefenseArea) { + if (skillStpInfo.getField().value().leftDefenseArea.contains(interceptionPosition)) { + std::vector intersections = FieldComputations::getDefenseArea(skillStpInfo.getField().value(), true, 0, 0) + .intersections({interceptionPosition, skillStpInfo.getBall().value()->expectedEndPosition}); + if (intersections.size() == 1) interceptionPosition = intersections.at(0); + } else if (skillStpInfo.getField().value().rightDefenseArea.contains(interceptionPosition)) { + std::vector intersections = FieldComputations::getDefenseArea(skillStpInfo.getField().value(), false, 0, 0) + .intersections({interceptionPosition, skillStpInfo.getBall().value()->expectedEndPosition}); + if (intersections.size() == 1) interceptionPosition = intersections.at(0); + } + } + + auto trajectory = Trajectory2D(robotPosition, robotVelocity, interceptionPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); + auto timeToTarget = trajectory.getTotalTime(); + if (timeToTarget < loopTime) { + break; + } + } + // distance to the ball at the time we intercept it + double distanceToInterception = (interceptionPosition - robotPosition).length() - control_constants::BALL_RADIUS - control_constants::ROBOT_RADIUS + + control_constants::GO_TO_POS_ERROR_MARGIN + 2 * control_constants::BALL_RADIUS; + // distance to the ball right now + double distanceToBall = (ballPosition - robotPosition).length() - control_constants::BALL_RADIUS - control_constants::ROBOT_RADIUS + control_constants::GO_TO_POS_ERROR_MARGIN + + 2 * control_constants::BALL_RADIUS; + + if (skillStpInfo.getRobot()->get()->getAngleDiffToBall() > Constants::HAS_BALL_ANGLE() && distanceToBall < control_constants::ROBOT_CLOSE_TO_POINT) { // don't move too close to the ball until the angle to the ball is (roughly) correct skillStpInfo.setPositionToMoveTo( FieldComputations::projectPointToValidPosition(info.getField().value(), skillStpInfo.getRobot()->get()->getPos(), info.getObjectsToAvoid())); } else { // We want to keep going towards the ball slowly if we are already close, to make sure we get it - auto getBallDistance = std::max(ballDistance, control_constants::ROBOT_RADIUS); - Vector2 newRobotPosition = robotPosition + (ballPosition - robotPosition).stretchToLength(getBallDistance); + auto getBallDistance = std::max(distanceToInterception, control_constants::ROBOT_RADIUS); + Vector2 newRobotPosition = robotPosition + (interceptionPosition - robotPosition).stretchToLength(getBallDistance); newRobotPosition = FieldComputations::projectPointToValidPosition(info.getField().value(), newRobotPosition, info.getObjectsToAvoid()); skillStpInfo.setPositionToMoveTo(newRobotPosition); } skillStpInfo.setAngle((ballPosition - robotPosition).angle()); - if (ballDistance < control_constants::TURN_ON_DRIBBLER_DISTANCE) { + if (distanceToBall < control_constants::TURN_ON_DRIBBLER_DISTANCE) { skillStpInfo.setDribblerSpeed(100); } diff --git a/roboteam_ai/src/utilities/Constants.cpp b/roboteam_ai/src/utilities/Constants.cpp index bf71cf561..e4b20ad0b 100644 --- a/roboteam_ai/src/utilities/Constants.cpp +++ b/roboteam_ai/src/utilities/Constants.cpp @@ -38,7 +38,7 @@ double Constants::MAX_DEC_LOWER() { return MAX_ACC_LOWER() * 1.2; } // magic nu double Constants::HAS_BALL_DISTANCE() { return (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? 0.11 : 0.12; } // The max angle the ball can have to the robot for the robot to have the ball -double Constants::HAS_BALL_ANGLE() { return 0.15 * M_PI; } +double Constants::HAS_BALL_ANGLE() { return 0.1 * M_PI; } std::map Constants::ROBOTS_WITH_WORKING_DRIBBLER() { static std::map workingDribblerRobots; diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index 49b623c7d..ead765b47 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -252,6 +252,7 @@ double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::Ro if (stpInfo.getPositionToShootAt().has_value()) target_position = world.getBall()->get()->position; if (stpInfo.getPositionToMoveTo().has_value()) target_position = stpInfo.getPositionToMoveTo().value(); // If robot is keeper, set distance to self. Basically 0 + // TODO: Is this if ever true? This is already handeled before right? if (stpInfo.getRoleName() == "keeper" && robot->getId() == GameStateManager::getCurrentGameState().keeperId) target_position = robot->getPos(); // No target found to move to @@ -261,14 +262,7 @@ double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::Ro RTT_WARNING("No target position found for role " + stpInfo.getRoleName() + " for robot " + std::to_string(robot->getId())) return 0; } - double dealer_speed_factor = stp::control_constants::DEALER_SPEED_FACTOR; - // if role is harasher - if (stpInfo.getRoleName() == "harasser") { - target_position = target_position.value() + world.getBall()->get()->velocity * dealer_speed_factor; - distance = (robot->getPos() + robot->getVel() * dealer_speed_factor).dist(*target_position); - } else { - distance = robot->getPos().dist(*target_position); - } + distance = robot->getPos().dist(*target_position); return costForDistance(distance, field->playArea.height()); } @@ -279,12 +273,6 @@ double Dealer::getDefaultFlagScores(const v::RobotView &robot, const Dealer::Dea // return costForProperty(robot->isWorkingBallSensor()); case DealerFlagTitle::WITH_WORKING_DRIBBLER: return costForProperty(robot->isWorkingDribbler()); - // case DealerFlagTitle::READY_TO_INTERCEPT_GOAL_SHOT: { - // get distance to line between ball and goal - // TODO this method can be improved by choosing a better line for the interception. - // LineSegment lineSegment = {world.getBall()->get()->position, field->leftGoalArea.rightLine().center()}; - // return lineSegment.distanceToLine(robot->getPos()); - // } case DealerFlagTitle::KEEPER: return costForProperty(robot->getId() == GameStateManager::getCurrentGameState().keeperId); case DealerFlagTitle::CAN_DETECT_BALL: { diff --git a/roboteam_ai/src/world/FieldComputations.cpp b/roboteam_ai/src/world/FieldComputations.cpp index 12e25cb51..d2be13ec2 100644 --- a/roboteam_ai/src/world/FieldComputations.cpp +++ b/roboteam_ai/src/world/FieldComputations.cpp @@ -2,6 +2,7 @@ #include +#include "utilities/GameSettings.h" #include "utilities/GameStateManager.hpp" #include "world/views/WorldDataView.hpp" @@ -33,6 +34,19 @@ bool FieldComputations::getBallAvoidance() { return false; } +Vector2 FieldComputations::getBallPositionAtTime(const rtt::world::ball::Ball &ball, double time) { + const double initialVelocity = ball.velocity.length(); + const double frictionCoefficient = + GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::stp::control_constants::SIMULATION_FRICTION : ai::stp::control_constants::REAL_FRICTION; + double timeToStop = initialVelocity / frictionCoefficient; + if (time > timeToStop) { + time = timeToStop; + } + double distance = initialVelocity * time - 0.5 * frictionCoefficient * time * time; + auto expectedEndPosition = ball.position + ball.velocity.stretchToLength(distance); + return expectedEndPosition; +} + bool FieldComputations::pointIsValidPosition(const rtt::Field &field, const Vector2 &point, stp::AvoidObjects avoidObjects, double fieldMargin) { auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = getDefenseAreaMargin(); if (avoidObjects.shouldAvoidOutOfField && !field.playArea.contains(point, fieldMargin)) return false; From fe9a333095e30b352a81cc52aa41370eb04a912c Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 26 Feb 2024 14:44:25 +0100 Subject: [PATCH 18/22] Add test for friction coefficient and fix GetBall when ball goes out of field --- .../stp/constants/ControlConstants.h | 4 +-- .../stp/computations/PositionComputations.cpp | 26 ++++++++++++++----- .../src/stp/tactics/active/GetBall.cpp | 8 +++--- roboteam_ai/src/world/Ball.cpp | 18 +++++++++++++ 4 files changed, 44 insertions(+), 12 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index ded53dcc8..b5ee45897 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -81,8 +81,8 @@ 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 */ /// Friction constants -constexpr static float SIMULATION_FRICTION = 0.69; /**< * The expected movement friction of the ball during simulation */ -constexpr static float REAL_FRICTION = 0.5; /**< The expected movement friction of the ball on the field */ +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 */ } // namespace rtt::ai::stp::control_constants diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index b0bb1a3c9..5d596b5e3 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -257,13 +257,15 @@ void PositionComputations::calculateInfoForKeeper(std::unordered_mapgetWorld()->getBall()->get()), loopTime); - + if (!field.playArea.contains(newBallPos, control_constants::BALL_RADIUS)) { + maximumTimeToIntercept = loopTime; + break; + } if (field.leftDefenseArea.contains(newBallPos)) { std::vector intersections = FieldComputations::getDefenseArea(field, true, 0, 0).intersections({newBallPos, world->getWorld()->getBall()->get()->expectedEndPosition}); @@ -277,13 +279,23 @@ HarasserInfo PositionComputations::calculateHarasserId(world::World *world, cons for (const auto &robot : world->getWorld()->getUs()) { if (robot->getId() == keeperId) continue; auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), newBallPos, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - auto timeToTarget = trajectory.getTotalTime(); - if (timeToTarget < loopTime) { + if (trajectory.getTotalTime() < loopTime) { return {robot->getId(), loopTime}; } } } - return {harasserId, endTime}; + double minTimeToTarget = std::numeric_limits::max(); + int minTimeRobotId; + for (const auto &robot : world->getWorld()->getUs()) { + if (robot->getId() == keeperId) continue; + auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), newBallPos, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); + auto timeToTarget = trajectory.getTotalTime(); + if (timeToTarget < minTimeToTarget) { + minTimeToTarget = timeToTarget; + minTimeRobotId = robot->getId(); + } + } + return {minTimeRobotId, maximumTimeToIntercept}; } void PositionComputations::calculateInfoForHarasser(std::unordered_map &stpInfos, diff --git a/roboteam_ai/src/stp/tactics/active/GetBall.cpp b/roboteam_ai/src/stp/tactics/active/GetBall.cpp index ae2e03901..ac75df9f7 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -32,8 +32,11 @@ std::optional GetBall::calculateInfoForSkill(StpInfo const &info) noexc maxRobotVelocity = std::clamp(skillStpInfo.getBall().value()->velocity.length() * 0.8, 0.5, maxRobotVelocity); skillStpInfo.setMaxRobotVelocity(maxRobotVelocity); } - for (double loopTime = 0; loopTime < 5; loopTime += 0.1) { + for (double loopTime = 0; loopTime < 1; loopTime += 0.1) { interceptionPosition = FieldComputations::getBallPositionAtTime(*(skillStpInfo.getCurrentWorld()->getWorld()->getBall()->get()), loopTime); + if (skillStpInfo.getObjectsToAvoid().shouldAvoidOutOfField && !skillStpInfo.getField().value().playArea.contains(interceptionPosition, control_constants::BALL_RADIUS)) { + break; + } if (skillStpInfo.getObjectsToAvoid().shouldAvoidDefenseArea) { if (skillStpInfo.getField().value().leftDefenseArea.contains(interceptionPosition)) { std::vector intersections = FieldComputations::getDefenseArea(skillStpInfo.getField().value(), true, 0, 0) @@ -47,8 +50,7 @@ std::optional GetBall::calculateInfoForSkill(StpInfo const &info) noexc } auto trajectory = Trajectory2D(robotPosition, robotVelocity, interceptionPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - auto timeToTarget = trajectory.getTotalTime(); - if (timeToTarget < loopTime) { + if (trajectory.getTotalTime() < loopTime) { break; } } diff --git a/roboteam_ai/src/world/Ball.cpp b/roboteam_ai/src/world/Ball.cpp index 3a4be119a..f1a392927 100644 --- a/roboteam_ai/src/world/Ball.cpp +++ b/roboteam_ai/src/world/Ball.cpp @@ -10,6 +10,10 @@ #include "world/World.hpp" namespace rtt::world::ball { +// Uncomment the following lines to calculate the friction coefficient +// static Vector2 maxVelocity; +// static Vector2 posAtMaxVelocity; +// static std::chrono::steady_clock::time_point timeOfLastReset; Ball::Ball(const proto::WorldBall& copy, const World* data) : position{copy.pos().x(), copy.pos().y()}, velocity{copy.vel().x(), copy.vel().y()}, visible{copy.visible()} { if (!visible || position == Vector2()) { @@ -51,6 +55,20 @@ void Ball::updateExpectedBallEndPosition(const world::World* data) noexcept { GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::stp::control_constants::SIMULATION_FRICTION : ai::stp::control_constants::REAL_FRICTION; expectedEndPosition = ball->position + ball->velocity.stretchToLength(ballVelSquared / frictionCoefficient); + // Uncomment the following lines to calculate the friction coefficient + // auto currentTime = std::chrono::steady_clock::now(); + // if (std::chrono::duration_cast(currentTime - timeOfLastReset).count() >= 30) { + // maxVelocity = Vector2(0, 0); + // posAtMaxVelocity = Vector2(0, 0); + // timeOfLastReset = currentTime; + // } + // if (velocity.length() > maxVelocity.length()) { + // maxVelocity = velocity; + // posAtMaxVelocity = position; + // } + // std::cout << "Expected friction coefficient: " << maxVelocity.length2() / (posAtMaxVelocity - ball->position).length() << std::endl; + // std::cout << "Time till next reset: " << 30.0 - std::chrono::duration(currentTime - timeOfLastReset).count() << std::endl; + std::array ballPoints = {expectedEndPosition, ball->position}; rtt::ai::gui::Out::draw( { From fadc56ee9f18e1e6adcb29f82627d88b6ff443af Mon Sep 17 00:00:00 2001 From: Luuk Date: Mon, 26 Feb 2024 15:08:53 +0100 Subject: [PATCH 19/22] Fix position of ball placer if the next state is force start --- .../stp/tactics/passive/BallStandBack.h | 2 ++ .../src/stp/tactics/passive/BallStandBack.cpp | 15 ++++++++++++--- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/tactics/passive/BallStandBack.h b/roboteam_ai/include/roboteam_ai/stp/tactics/passive/BallStandBack.h index 9a6e2558b..8d0daaa94 100644 --- a/roboteam_ai/include/roboteam_ai/stp/tactics/passive/BallStandBack.h +++ b/roboteam_ai/include/roboteam_ai/stp/tactics/passive/BallStandBack.h @@ -55,6 +55,8 @@ class BallStandBack : public Tactic { * @return The name of this tactic */ const char *getName() override; + + bool standBack; }; } // namespace rtt::ai::stp::tactic diff --git a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp index 6d7dddcbd..6fd7d3118 100644 --- a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp @@ -20,16 +20,25 @@ std::optional BallStandBack::calculateInfoForSkill(StpInfo const &info) if (!info.getPositionToMoveTo() || !skillStpInfo.getBall() || !skillStpInfo.getRobot()) return std::nullopt; RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); Vector2 targetPos; + Vector2 ballTarget = info.getBall()->get()->position; if (standStillCounter > 60) { - auto moveVector = info.getRobot()->get()->getPos() - info.getBall()->get()->position; + auto moveVector = info.getRobot()->get()->getPos() - ballTarget; double stretchLength = (currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) ? control_constants::AVOID_BALL_DISTANCE_BEFORE_FREE_KICK : control_constants::AVOID_BALL_DISTANCE; - targetPos = info.getBall()->get()->position + moveVector.stretchToLength(stretchLength); + targetPos = ballTarget + moveVector.stretchToLength(stretchLength); + if (((info.getRobot()->get()->getPos() - targetPos).length() < control_constants::GO_TO_POS_ERROR_MARGIN || standBack == true) && + currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { + targetPos = ballTarget + (skillStpInfo.getField().value().leftGoalArea.leftLine().center() - ballTarget).stretchToLength(control_constants::AVOID_BALL_DISTANCE); + standBack = true; + skillStpInfo.setShouldAvoidBall(true); + } } else { + standBack = false; standStillCounter++; targetPos = info.getRobot()->get()->getPos(); + skillStpInfo.setShouldAvoidBall(false); } - skillStpInfo.setShouldAvoidBall(false); + double angle = (info.getBall()->get()->position - targetPos).angle(); skillStpInfo.setPositionToMoveTo(targetPos); skillStpInfo.setAngle(angle); From 28b708bac33431a0d2a82934d1c73f3e6e70c39e Mon Sep 17 00:00:00 2001 From: Luuk Date: Mon, 26 Feb 2024 16:24:50 +0100 Subject: [PATCH 20/22] Add option to disable margins in the interface --- roboteam_interface/src/generated/proto.d.ts | 3 +- roboteam_interface/src/generated/proto.js | 6 ++++ .../components/canvas/visualizations.vue | 1 + .../components/ui-settings/dual-state.vue | 30 +++++++++++++++++++ .../components/ui-settings/ui-settings.vue | 6 ++++ .../src/modules/stores/ui-store.ts | 9 +++++- roboteam_networking/proto/GUI.proto | 1 + 7 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 roboteam_interface/src/modules/components/ui-settings/dual-state.vue diff --git a/roboteam_interface/src/generated/proto.d.ts b/roboteam_interface/src/generated/proto.d.ts index 93a3e978a..a65f57cbb 100644 --- a/roboteam_interface/src/generated/proto.d.ts +++ b/roboteam_interface/src/generated/proto.d.ts @@ -174,7 +174,8 @@ export namespace proto { /** Category enum. */ enum Category { PATH_PLANNING = 0, - DEBUG = 1 + DEBUG = 1, + MARGINS = 2 } } diff --git a/roboteam_interface/src/generated/proto.js b/roboteam_interface/src/generated/proto.js index d4ab6a3f8..15d4390df 100644 --- a/roboteam_interface/src/generated/proto.js +++ b/roboteam_interface/src/generated/proto.js @@ -440,6 +440,10 @@ export const proto = $root.proto = (() => { case 1: message.category = 1; break; + case "MARGIN": + case 2: + message.category = 2; + break; } if (object.forRobotId != null) message.forRobotId = object.forRobotId >>> 0; @@ -577,11 +581,13 @@ export const proto = $root.proto = (() => { * @enum {number} * @property {number} PATH_PLANNING=0 PATH_PLANNING value * @property {number} DEBUG=1 DEBUG value + * @property {number} MARGINS=2 MARGINS value */ Drawing.Category = (function() { const valuesById = {}, values = Object.create(valuesById); values[valuesById[0] = "PATH_PLANNING"] = 0; values[valuesById[1] = "DEBUG"] = 1; + values[valuesById[2] = "MARGINS"] = 2; return values; })(); diff --git a/roboteam_interface/src/modules/components/canvas/visualizations.vue b/roboteam_interface/src/modules/components/canvas/visualizations.vue index b05d5b2ad..23f54a4f4 100644 --- a/roboteam_interface/src/modules/components/canvas/visualizations.vue +++ b/roboteam_interface/src/modules/components/canvas/visualizations.vue @@ -25,6 +25,7 @@ const onPixiTick = () => { if (data.category == Category.PATH_PLANNING && !uiStore.showPathPlanning(data.forRobotId)) return if (data.category == Category.DEBUG && !uiStore.showDebug(data.forRobotId)) return + if (data.category == Category.MARGINS && !uiStore.showMargins(data.forRobotId)) return const shape = new ShapeDrawing({ data: data as NoUndefinedField, diff --git a/roboteam_interface/src/modules/components/ui-settings/dual-state.vue b/roboteam_interface/src/modules/components/ui-settings/dual-state.vue new file mode 100644 index 000000000..3fe4f234e --- /dev/null +++ b/roboteam_interface/src/modules/components/ui-settings/dual-state.vue @@ -0,0 +1,30 @@ + + + \ No newline at end of file diff --git a/roboteam_interface/src/modules/components/ui-settings/ui-settings.vue b/roboteam_interface/src/modules/components/ui-settings/ui-settings.vue index f0027ec71..9e71fe616 100644 --- a/roboteam_interface/src/modules/components/ui-settings/ui-settings.vue +++ b/roboteam_interface/src/modules/components/ui-settings/ui-settings.vue @@ -1,6 +1,7 @@