From ad27b7112aeb4a9727e656ed8b7b6c5b73b358b1 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 9 Jan 2024 19:11:13 +0100 Subject: [PATCH 01/17] Change path planning scoring --- .../BBTrajectories/WorldObjects.h | 26 +++- .../control/positionControl/PositionControl.h | 9 -- .../BBTrajectories/WorldObjects.cpp | 69 +++++++-- .../positionControl/PositionControl.cpp | 141 +++++++++++++----- roboteam_utils/include/roboteam_utils/Line.h | 8 + roboteam_utils/src/utils/Line.cpp | 19 +++ 6 files changed, 211 insertions(+), 61 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h index 1bb4265e3..91b788d7c 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h @@ -16,13 +16,11 @@ namespace rtt::BB { /** * @brief Struct that stores data about a collision - * @memberof obstaclePosition position of the obstacle * @memberof collisionPosition position robot shouldn't come * @memberof collisionTime number of seconds from now that the collision will occur * @memberof collisionName the name of what causes the collision */ struct CollisionData { - Vector2 obstaclePosition; Vector2 collisionPosition; double collisionTime; std::string collisionName; @@ -64,6 +62,17 @@ class WorldObjects { std::optional getFirstCollision(const rtt::world::World *world, const rtt::Field &field, const rtt::Trajectory2D &Trajectory, const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects); + /** + * @brief Calculates the position of the first collision with the defense area + * @param field used for checking collisions with the field + * @param BBTrajectory the trajectory to check for collisions + * @param computedPaths the paths of our robots + * @param robotId Id of the robot + * @return optional with rtt::BB::CollisionData + */ + std::optional getFirstDefenseAreaCollision(const rtt::Field &field, const rtt::Trajectory2D &Trajectory, + const std::unordered_map> &computedPaths, int robotId); + /** * @brief Takes a calculated path of a robot and checks points along the path if they are outside the * fieldlines if not allowed there. Adds these points and the time to collisionDatas and collisionTimes @@ -131,6 +140,19 @@ class WorldObjects { void calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, const std::unordered_map> &computedPaths, int robotId, double timeStep, size_t completedTimeSteps); + /** + * @brief Takes a calculated path of a robot and checks points along the path if they are too close to an + * approximation of the ball trajactory. If the play is "ball_placement_them" also checks for the path + * being inside the balltube. Adds these points and the time to collisionDatas and collisionTimes + * @param world Used for information about the ball + * @param collisionDatas std::vector which rtt::BB::CollisionData can be added to + * @param pathPoints std::vector with path points + * @param timeStep Time between pathpoints + * @param completedTimeSteps Number of completed time steps + */ + void calculateBallPlacementCollision(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, + size_t completedTimeSteps); + /** * @brief Inserts collisionData in the vector collisionDatas such that they are ordered from lowest collisionTime to highest * @param collisionDatas std::vector which the collisionData needs to be added to diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index 7adb302ba..038d28b18 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -150,15 +150,6 @@ class PositionControl { * @return A vector with coordinates of the intermediate points */ std::vector createIntermediatePoints(const rtt::Field &field, int robotId, std::optional &firstCollision, Vector2 &targetPosition); - - /** - * @brief Gives each intermediate point a score for how close the point is to the collisionPosition - * @param intermediatePoints the intermediate points for trying to find a new path - * @param firstCollision used for scoring the points - * @return A priority_queue to sort the points - */ - std::priority_queue, std::vector>, std::greater<>> scoreIntermediatePoints( - std::vector &intermediatePoints, std::optional &firstCollision); }; } // namespace rtt::ai::control diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 5b849d8cb..4cf710835 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -5,6 +5,7 @@ #include +#include "stp/constants/ControlConstants.h" #include "world/World.hpp" #include "world/WorldData.hpp" @@ -14,9 +15,6 @@ WorldObjects::WorldObjects() = default; std::optional WorldObjects::getFirstCollision(const rtt::world::World *world, const rtt::Field &field, const Trajectory2D &Trajectory, const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects) { - // Get the current game state - gameState = rtt::ai::GameStateManager::getCurrentGameState(); - // Set the time step for the trajectory calculation double timeStep = 0.1; @@ -50,6 +48,38 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W if (avoidObjects.shouldAvoidOurRobots) { calculateOurRobotCollisions(world, collisionDatas, pathPoints, computedPaths, robotId, timeStep, completedTimeSteps); } + if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { + calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps); + } + if (!collisionDatas.empty()) { + return collisionDatas[0]; + } else { + return std::nullopt; + } +} + +std::optional WorldObjects::getFirstDefenseAreaCollision(const rtt::Field &field, const Trajectory2D &Trajectory, + const std::unordered_map> &computedPaths, int robotId) { + // Set the time step for the trajectory calculation + double timeStep = 0.1; + + // Calculate the points on the trajectory at intervals of `timeStep` + auto pathPoints = Trajectory.getPathApproach(timeStep); + + // If the computed path for the robot exists and has more than 10 points, + // and `pathPoints` also has more than 10 points, remove the last 3 points from `pathPoints`. + // This avoids checking for collisions at the endpoint of the trajectory. + if (computedPaths.contains(robotId) && computedPaths.at(robotId).size() > 10 && pathPoints.size() > 10) { + pathPoints.erase(pathPoints.end() - 3, pathPoints.end()); + } + + // Calculate the number of time steps that have been completed + size_t completedTimeSteps = computedPaths.contains(robotId) && pathPoints.size() > computedPaths.at(robotId).size() ? pathPoints.size() - computedPaths.at(robotId).size() : 0; + + std::vector collisionDatas; + + calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep, completedTimeSteps); + if (!collisionDatas.empty()) { return collisionDatas[0]; } else { @@ -64,7 +94,7 @@ void WorldObjects::calculateFieldCollisions(const rtt::Field &field, std::vector // Don't care about the field if the robot is already outside the field (i == 0 is the first point of the robot's path, so almost the currentPosition). if (i == 0) return; - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], pathPoints[i], i * timeStep, "FieldCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "FieldCollision"}); return; } } @@ -73,15 +103,15 @@ void WorldObjects::calculateFieldCollisions(const rtt::Field &field, std::vector void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, int robotId, double timeStep, size_t completedTimeSteps) { auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = rtt::ai::FieldComputations::getDefenseAreaMargin(); - auto ourDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 0); - auto theirDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, false, theirDefenseAreaMargin, 0); + auto ourDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1); + auto theirDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, false, theirDefenseAreaMargin, 1); for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { if (ourDefenseArea.contains(pathPoints[i]) || theirDefenseArea.contains(pathPoints[i])) { // Don't care about the defense area if the robot is already in the defense area. It should just get out as fast as possible :) // if (i == 0) return; - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], pathPoints[i], i * timeStep, "DefenseAreaCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "DefenseAreaCollision"}); return; } } @@ -103,7 +133,7 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: for (size_t i = completedTimeSteps; i < ballTrajectory.size(); i++) { if (pathPoints[i].dist(ballTrajectory[i]) < dist) { - insertCollisionData(collisionDatas, CollisionData{ballTrajectory[i], pathPoints[i], i * timeStep, "BallCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "BallCollision"}); return; } } @@ -132,7 +162,7 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, Vector2 posDif = interpolatedPoint - theirPos; if (posDif.length() < avoidanceDistance) { - insertCollisionData(collisionDatas, CollisionData{theirPos, interpolatedPoint, interpolatedTime, "EnemyRobotCollision"}); + insertCollisionData(collisionDatas, CollisionData{interpolatedPoint, interpolatedTime, "EnemyRobotCollision"}); return; } } @@ -147,7 +177,7 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s // Because wallers are irritating and colide with each other, we ignore first 10 timesteps // Otherwise, we get collisions in every direction and we just yoink through our own defense are // TODO: Maybe fix this in a better way if we get a lot of collisions. - for (size_t i = completedTimeSteps + 10; i < pathPoints.size() - 1; i++) { + for (size_t i = completedTimeSteps + 6; i < pathPoints.size() - 1; i++) { double currentTime = i * timeStep; if (currentTime - completedTimeSteps * timeStep >= maxCollisionCheckTime) break; @@ -168,7 +198,7 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s } if ((interpolatedPoint - theirPos).length() < avoidanceDistance) { - insertCollisionData(collisionDatas, CollisionData{theirPos, interpolatedPoint, interpolatedTime, "OurRobotCollision"}); + insertCollisionData(collisionDatas, CollisionData{interpolatedPoint, interpolatedTime, "OurRobotCollision"}); return; } } @@ -176,6 +206,23 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s } } } + +void WorldObjects::calculateBallPlacementCollision(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, + double timeStep, size_t completedTimeSteps) { + auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); + auto startPositionBall = world->getWorld()->getBall()->get()->position; + // line segment from startPosition to ballPlacementPos + LineSegment ballPlacementLine(startPositionBall, ballPlacementPos); + double time = completedTimeSteps * timeStep; + + for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { + if (ballPlacementLine.distanceToLine(pathPoints[i]) < 0.4) { + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "BallPlacementCollision"}); + return; + } + } +} + // Insert a collision data object into a vector of collision data objects, sorted by collision time void WorldObjects::insertCollisionData(std::vector &collisionDatas, const CollisionData &collisionData) { collisionDatas.insert(std::upper_bound(collisionDatas.begin(), collisionDatas.end(), collisionData, diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 555a17275..584e54822 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -7,6 +7,7 @@ #include "control/positionControl/BBTrajectories/BBTrajectory2D.h" #include "gui/Out.h" #include "roboteam_utils/Print.h" +#include "world/World.hpp" namespace rtt::ai::control { std::vector PositionControl::getComputedPath(int ID) { return computedPaths[ID]; } @@ -122,26 +123,69 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: Vector2 ¤tVelocity, std::optional &firstCollision, Vector2 &targetPosition, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { auto intermediatePoints = createIntermediatePoints(field, robotId, firstCollision, targetPosition); - auto intermediatePointsSorted = scoreIntermediatePoints(intermediatePoints, firstCollision); + // draw intermediate points on the field for debugging + rtt::ai::gui::Out::draw( + { + .label = "intermediate_points" + std::to_string(robotId), + .color = proto::Drawing::YELLOW, + .method = proto::Drawing::DOTS, + .category = proto::Drawing::PATH_PLANNING, + .forRobotId = robotId, + .size = 2, + }, + intermediatePoints); - double longestTimeTillCollision = 0; + double bestScore = 999; std::optional bestTrajectory = std::nullopt; - while (!intermediatePointsSorted.empty()) { - Trajectory2D trajectoryToIntermediatePoint(currentPosition, currentVelocity, intermediatePointsSorted.top().second, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); + for (const auto &intermediatePoint : intermediatePoints) { + Trajectory2D trajectoryToIntermediatePoint(currentPosition, currentVelocity, intermediatePoint, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); auto intermediatePathCollision = worldObjects.getFirstCollision(world, field, trajectoryToIntermediatePoint, computedPaths, robotId, avoidObjects); auto trajectoryAroundCollision = calculateTrajectoryAroundCollision(world, field, intermediatePathCollision, trajectoryToIntermediatePoint, targetPosition, robotId, maxRobotVelocity, timeStep, avoidObjects); if (trajectoryAroundCollision.has_value()) { auto firstCollision = worldObjects.getFirstCollision(world, field, trajectoryAroundCollision.value(), computedPaths, robotId, avoidObjects); - double timeTillCollision = firstCollision.has_value() ? firstCollision.value().collisionTime : trajectoryAroundCollision.value().getTotalTime(); - if (timeTillCollision > longestTimeTillCollision) { - longestTimeTillCollision = timeTillCollision; + double totalTime = trajectoryAroundCollision.value().getTotalTime(); + double score = totalTime; + if (firstCollision.has_value()) { + score += 5; + double timeTillFirstCollision = firstCollision.value().collisionTime; + score += std::max(0.0, 3 - timeTillFirstCollision); + // if the collision is with the defense area and within 1 seconds, add 5 to the score + // if the collision is with the defense area and within 1 seconds, add 5 to the score + if (avoidObjects.shouldAvoidDefenseArea) { + auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, trajectoryAroundCollision.value(), computedPaths, robotId); + if (defenseAreaCollision.has_value() && defenseAreaCollision.value().collisionTime < 1) { + score += 5; + } + } + + if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { + auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); + auto startPositionBall = world->getWorld()->getBall()->get()->position; + Line ballPlacementLine(startPositionBall, ballPlacementPos); + Vector2 p1 = firstCollision.value().collisionPosition; + Vector2 p2 = targetPosition; + if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { + double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); + double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); + if (field.leftDefenseArea.contains(ballPlacementPos, 1.5) || field.rightDefenseArea.contains(ballPlacementPos, 1.5)) { + d1 += 999; + } + if (field.leftDefenseArea.contains(startPositionBall, 1.5) || field.rightDefenseArea.contains(startPositionBall, 1.5)) { + d2 += 999; + } + score += std::min(d1, d2) * 10; + score += 10; + } + } + } + if (score < bestScore) { + bestScore = score; bestTrajectory = trajectoryAroundCollision.value(); } } - intermediatePointsSorted.pop(); } return bestTrajectory; } @@ -150,10 +194,10 @@ std::optional PositionControl::calculateTrajectoryAroundCollision( std::optional &intermediatePathCollision, Trajectory2D trajectoryToIntermediatePoint, Vector2 &targetPosition, int robotId, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { - double longestTimeTillCollision = 0; - std::optional longestTrajectory = std::nullopt; - std::optional longestIntermediateToTarget = std::nullopt; - double longestTime = 0; + double bestScore = 999; + double bestTime = 0; + std::optional bestTrajectory = std::nullopt; + std::optional bestIntermediateToTarget = std::nullopt; if (!intermediatePathCollision.has_value()) { timeStep *= 2; @@ -167,25 +211,56 @@ std::optional PositionControl::calculateTrajectoryAroundCollision( auto newStartCollisions = worldObjects.getFirstCollision(world, field, intermediateToTarget, computedPaths, robotId, avoidObjects); if (newStartCollisions.has_value()) { - double timeTillCollision = newStartCollisions.value().collisionTime; + double totalTime = i * timeStep + intermediateToTarget.getTotalTime(); + double timeTillFirstCollision = newStartCollisions.value().collisionTime; + double score = totalTime + 5 + std::max(0.0, 3 - timeTillFirstCollision); + // if the collision is with the defense area and within 1 seconds, add 5 to the score + if (avoidObjects.shouldAvoidDefenseArea) { + auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, intermediateToTarget, computedPaths, robotId); + if (defenseAreaCollision.has_value() && defenseAreaCollision.value().collisionTime < 1) { + score += 5; + } + } + + if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { + auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); + auto startPositionBall = world->getWorld()->getBall()->get()->position; + Line ballPlacementLine(startPositionBall, ballPlacementPos); + Vector2 p1 = newStartCollisions.value().collisionPosition; + Vector2 p2 = targetPosition; + if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { + double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); + double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); + if (field.leftDefenseArea.contains(ballPlacementPos, 1.5) || field.rightDefenseArea.contains(ballPlacementPos, 1.5)) { + d1 += 999; + } + if (field.leftDefenseArea.contains(startPositionBall, 1.5) || field.rightDefenseArea.contains(startPositionBall, 1.5)) { + d2 += 999; + } + d2 += 999; + score += std::min(d1, d2) * 10; + score += 10; + } + } - if (timeTillCollision > longestTimeTillCollision) { - longestTimeTillCollision = timeTillCollision; - longestTrajectory = trajectoryToIntermediatePoint; - longestIntermediateToTarget = intermediateToTarget; - longestTime = i * timeStep; + if (score < bestScore) { + bestScore = score; + bestTrajectory = trajectoryToIntermediatePoint; + bestIntermediateToTarget = intermediateToTarget; + bestTime = i * timeStep; } } else { trajectoryToIntermediatePoint.addTrajectory(intermediateToTarget, i * timeStep); + trajectoryToIntermediatePoint.getTotalTime(); return trajectoryToIntermediatePoint; // This is now the whole path } } } - // If there was a collision in all intermediate path, return the longest collision free trajectory - if (longestTrajectory.has_value()) { - longestTrajectory.value().addTrajectory(longestIntermediateToTarget.value(), longestTime); + // If there was a collision in all intermediate path, return the best free trajectory + if (bestTrajectory.has_value()) { + bestTrajectory.value().addTrajectory(bestIntermediateToTarget.value(), bestTime); } - return longestTrajectory; + return bestTrajectory; } std::vector PositionControl::createIntermediatePoints(const rtt::Field &field, int robotId, std::optional &firstCollision, Vector2 &targetPosition) { @@ -196,16 +271,16 @@ std::vector PositionControl::createIntermediatePoints(const rtt::Field // PointToDrawFrom is picked by drawing a line from the target position to the obstacle and extending that // line further towards our currentPosition by extension meters. - float pointExtension = fieldHeight / 18; // How far the pointToDrawFrom has to be from the obstaclePosition - Vector2 pointToDrawFrom = firstCollision->obstaclePosition + (firstCollision->obstaclePosition - targetPosition).normalize() * pointExtension; + float pointExtension = fieldHeight / 18; // How far the pointToDrawFrom has to be from the collisionPosition + Vector2 pointToDrawFrom = firstCollision->collisionPosition + (firstCollision->collisionPosition - targetPosition).normalize() * pointExtension; std::vector intermediatePoints; - for (int i = -4; i < 5; i++) { + for (int i = -9; i < 9; i++) { if (i != 0) { - // Make half circle of intermediatePoints pointed towards obstaclePosition, originating from pointToDrawFrom, by rotating pointToRotate with a radius + // Make half circle of intermediatePoints pointed towards collisionPosition, originating from pointToDrawFrom, by rotating pointToRotate with a radius // intermediatePointRadius float intermediatePointRadius = fieldHeight / 4; // Radius of the half circle - Vector2 pointToRotate = pointToDrawFrom + (targetPosition - firstCollision->obstaclePosition).normalize() * intermediatePointRadius; + Vector2 pointToRotate = pointToDrawFrom + (targetPosition - firstCollision->collisionPosition).normalize() * intermediatePointRadius; Vector2 intermediatePoint = pointToRotate.rotateAroundPoint(i * angleBetweenIntermediatePoints, pointToDrawFrom); if (!field.playArea.contains(intermediatePoint)) { @@ -217,18 +292,6 @@ std::vector PositionControl::createIntermediatePoints(const rtt::Field return intermediatePoints; } -std::priority_queue, std::vector>, std::greater<>> PositionControl::scoreIntermediatePoints( - std::vector &intermediatePoints, std::optional &firstCollision) { - double intermediatePointScore; - std::priority_queue, std::vector>, std::greater<>> intermediatePointsSorted; - for (const auto &i : intermediatePoints) { - intermediatePointScore = (i - firstCollision->collisionPosition).length(); - std::pair p = {intermediatePointScore, i}; - intermediatePointsSorted.push(p); - } - return intermediatePointsSorted; -} - bool PositionControl::shouldRecalculateTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 targetPosition, const Vector2 ¤tPosition, ai::stp::AvoidObjects avoidObjects) { if (!computedTrajectories.contains(robotId) || diff --git a/roboteam_utils/include/roboteam_utils/Line.h b/roboteam_utils/include/roboteam_utils/Line.h index 22e2644de..ed8b9d857 100644 --- a/roboteam_utils/include/roboteam_utils/Line.h +++ b/roboteam_utils/include/roboteam_utils/Line.h @@ -68,6 +68,14 @@ class Line { */ [[nodiscard]] bool isOnLine(const Vector2 &point) const; + /** + * Determine whether two points lie on opposite sides of this Line. + * @param point1 The first point. + * @param point2 The second point. + * @return True if the two points lie on opposite sides of this Line, false otherwise. + */ + [[nodiscard]] bool arePointsOnOppositeSides(const Vector2 &point1, const Vector2 &point2) const; + /** * Get the intersection point between two infinite lines. No intersection point is returned in case the lines are equal, parallel or if one of the lines is undefined * (a line is undefined if the 2 given points, which define the line, are equal). See https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection for more information. diff --git a/roboteam_utils/src/utils/Line.cpp b/roboteam_utils/src/utils/Line.cpp index d52d88ebd..7b0ed2f67 100644 --- a/roboteam_utils/src/utils/Line.cpp +++ b/roboteam_utils/src/utils/Line.cpp @@ -41,6 +41,19 @@ bool Line::isOnLine(const Vector2 &point) const { return fabs(A.cross(B)) < RTT_PRECISION_LIMIT; } +bool Line::arePointsOnOppositeSides(const Vector2 &point1, const Vector2 &point2) const { + Vector2 lineVector = v1 - v2; // vector along the line + Vector2 vectorFromLineStartToPoint1 = point1 - v1; // vector from line start to point 1 + Vector2 vectorFromLineStartToPoint2 = point2 - v1; // vector from line start to point 2 + + // cross product of lineVector and vectorFromLineStartToPoint1, and lineVector and vectorFromLineStartToPoint2 + double crossProduct1 = lineVector.x * vectorFromLineStartToPoint1.y - lineVector.y * vectorFromLineStartToPoint1.x; + double crossProduct2 = lineVector.x * vectorFromLineStartToPoint2.y - lineVector.y * vectorFromLineStartToPoint2.x; + + // if the cross products have different signs, the points are on opposite sides of the line + return crossProduct1 * crossProduct2 < 0; +} + std::optional Line::intersect(const Line &line) const { auto result = intersect(v1, v2, line.v1, line.v2); if (result.has_value()) { @@ -52,6 +65,12 @@ std::optional Line::intersect(const Line &line) const { } } +// // Move to be function of line class +// // Some dark mathemagic to check if two points are on opposite sides of a line +// bool arePointsOnOppositeSides(const Line &line, const Vector2 &p1, const Vector2 &p2) { + +// } + std::optional Line::intersect(const Vector2 p1, const Vector2 p2, const Vector2 q1, const Vector2 q2) { Vector2 A = p1 - p2; Vector2 B = q1 - q2; From 2aa50071313a458cc03cf3a00a094bedad947f41 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 12 Jan 2024 15:21:31 +0100 Subject: [PATCH 02/17] Improve path scoring --- .../stp/constants/ControlConstants.h | 1 + .../BBTrajectories/WorldObjects.cpp | 2 +- .../positionControl/PositionControl.cpp | 74 +++++++++++++------ roboteam_utils/src/utils/Line.cpp | 12 +-- 4 files changed, 54 insertions(+), 35 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index f8efbd460..06dd1dc6e 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -75,6 +75,7 @@ constexpr double DISTANCE_FROM_GOAL_CLOSE = 2 * ROBOT_RADIUS; /**< Distance from /// GameState constants constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS; /**< Minimum distance all robots should keep when avoiding the ball */ +constexpr double MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT = AVOID_BALL_DISTANCE + 2 * ROBOT_RADIUS + 0.4; /**< Minimum distance between the defense area and the ball placement position to be able to pass */ } // namespace rtt::ai::stp::control_constants diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 4cf710835..26376bcb3 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -216,7 +216,7 @@ void WorldObjects::calculateBallPlacementCollision(const rtt::world::World *worl double time = completedTimeSteps * timeStep; for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { - if (ballPlacementLine.distanceToLine(pathPoints[i]) < 0.4) { + if (ballPlacementLine.distanceToLine(pathPoints[i]) < ai::stp::control_constants::AVOID_BALL_DISTANCE) { insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "BallPlacementCollision"}); return; } diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 584e54822..c09a65732 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -123,17 +123,17 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: Vector2 ¤tVelocity, std::optional &firstCollision, Vector2 &targetPosition, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { auto intermediatePoints = createIntermediatePoints(field, robotId, firstCollision, targetPosition); - // draw intermediate points on the field for debugging - rtt::ai::gui::Out::draw( - { - .label = "intermediate_points" + std::to_string(robotId), - .color = proto::Drawing::YELLOW, - .method = proto::Drawing::DOTS, - .category = proto::Drawing::PATH_PLANNING, - .forRobotId = robotId, - .size = 2, - }, - intermediatePoints); + // draw intermediate points on the field, uncomment for debugging + // rtt::ai::gui::Out::draw( + // { + // .label = "intermediate_points" + std::to_string(robotId), + // .color = proto::Drawing::YELLOW, + // .method = proto::Drawing::DOTS, + // .category = proto::Drawing::PATH_PLANNING, + // .forRobotId = robotId, + // .size = 2, + // }, + // intermediatePoints); double bestScore = 999; std::optional bestTrajectory = std::nullopt; @@ -152,15 +152,12 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: score += 5; double timeTillFirstCollision = firstCollision.value().collisionTime; score += std::max(0.0, 3 - timeTillFirstCollision); - // if the collision is with the defense area and within 1 seconds, add 5 to the score - // if the collision is with the defense area and within 1 seconds, add 5 to the score if (avoidObjects.shouldAvoidDefenseArea) { auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, trajectoryAroundCollision.value(), computedPaths, robotId); - if (defenseAreaCollision.has_value() && defenseAreaCollision.value().collisionTime < 1) { - score += 5; + if (defenseAreaCollision.has_value()) { + score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime); } } - if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); auto startPositionBall = world->getWorld()->getBall()->get()->position; @@ -170,10 +167,12 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); - if (field.leftDefenseArea.contains(ballPlacementPos, 1.5) || field.rightDefenseArea.contains(ballPlacementPos, 1.5)) { + if (field.leftDefenseArea.contains(ballPlacementPos, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT) || + field.rightDefenseArea.contains(ballPlacementPos, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT)) { d1 += 999; } - if (field.leftDefenseArea.contains(startPositionBall, 1.5) || field.rightDefenseArea.contains(startPositionBall, 1.5)) { + if (field.leftDefenseArea.contains(startPositionBall, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT) || + field.rightDefenseArea.contains(startPositionBall, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT)) { d2 += 999; } score += std::min(d1, d2) * 10; @@ -181,6 +180,34 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: } } } + // plot the trajectory, uncomment for debugging + // double max_score = 50; + // double min_score = 0; + // score = (score - min_score) / (max_score - min_score); + // // if score is below 0.1, color is green, if score is above 0.5, color is red, else color is yellow + // auto color = score < 0.1 ? proto::Drawing::GREEN : score > 0.5 ? proto::Drawing::RED : proto::Drawing::YELLOW; + + // std::vector plotableTrajectory = trajectoryAroundCollision.value().getPathApproach(timeStep); + // rtt::ai::gui::Out::draw( + // { + // .label = "intermediate_trajectory" + std::to_string(robotId) + "_" + std::to_string(intermediatePoint.x) + "_" + std::to_string(intermediatePoint.y), + // .color = color, + // .method = proto::Drawing::LINES_CONNECTED, + // .category = proto::Drawing::PATH_PLANNING, + // .forRobotId = robotId, + // .thickness = std::max(int(1/score - 9), 0), + // }, + // plotableTrajectory); + // rtt::ai::gui::Out::draw( + // { + // .label = "intermediate_trajectory_dots" + std::to_string(robotId) + "_" + std::to_string(intermediatePoint.x) + "_" + std::to_string(intermediatePoint.y), + // .color = color, + // .method = proto::Drawing::DOTS, + // .category = proto::Drawing::PATH_PLANNING, + // .forRobotId = robotId, + // .size = 2, + // }, + // plotableTrajectory); if (score < bestScore) { bestScore = score; bestTrajectory = trajectoryAroundCollision.value(); @@ -214,11 +241,10 @@ std::optional PositionControl::calculateTrajectoryAroundCollision( double totalTime = i * timeStep + intermediateToTarget.getTotalTime(); double timeTillFirstCollision = newStartCollisions.value().collisionTime; double score = totalTime + 5 + std::max(0.0, 3 - timeTillFirstCollision); - // if the collision is with the defense area and within 1 seconds, add 5 to the score if (avoidObjects.shouldAvoidDefenseArea) { auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, intermediateToTarget, computedPaths, robotId); - if (defenseAreaCollision.has_value() && defenseAreaCollision.value().collisionTime < 1) { - score += 5; + if (defenseAreaCollision.has_value()) { + score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime); } } @@ -231,10 +257,12 @@ std::optional PositionControl::calculateTrajectoryAroundCollision( if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); - if (field.leftDefenseArea.contains(ballPlacementPos, 1.5) || field.rightDefenseArea.contains(ballPlacementPos, 1.5)) { + if (field.leftDefenseArea.contains(ballPlacementPos, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT) || + field.rightDefenseArea.contains(ballPlacementPos, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT)) { d1 += 999; } - if (field.leftDefenseArea.contains(startPositionBall, 1.5) || field.rightDefenseArea.contains(startPositionBall, 1.5)) { + if (field.leftDefenseArea.contains(startPositionBall, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT) || + field.rightDefenseArea.contains(startPositionBall, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT)) { d2 += 999; } d2 += 999; diff --git a/roboteam_utils/src/utils/Line.cpp b/roboteam_utils/src/utils/Line.cpp index 7b0ed2f67..58ea031a6 100644 --- a/roboteam_utils/src/utils/Line.cpp +++ b/roboteam_utils/src/utils/Line.cpp @@ -46,12 +46,8 @@ bool Line::arePointsOnOppositeSides(const Vector2 &point1, const Vector2 &point2 Vector2 vectorFromLineStartToPoint1 = point1 - v1; // vector from line start to point 1 Vector2 vectorFromLineStartToPoint2 = point2 - v1; // vector from line start to point 2 - // cross product of lineVector and vectorFromLineStartToPoint1, and lineVector and vectorFromLineStartToPoint2 - double crossProduct1 = lineVector.x * vectorFromLineStartToPoint1.y - lineVector.y * vectorFromLineStartToPoint1.x; - double crossProduct2 = lineVector.x * vectorFromLineStartToPoint2.y - lineVector.y * vectorFromLineStartToPoint2.x; - // if the cross products have different signs, the points are on opposite sides of the line - return crossProduct1 * crossProduct2 < 0; + return lineVector.cross(vectorFromLineStartToPoint1) * lineVector.cross(vectorFromLineStartToPoint2) < 0; } std::optional Line::intersect(const Line &line) const { @@ -65,12 +61,6 @@ std::optional Line::intersect(const Line &line) const { } } -// // Move to be function of line class -// // Some dark mathemagic to check if two points are on opposite sides of a line -// bool arePointsOnOppositeSides(const Line &line, const Vector2 &p1, const Vector2 &p2) { - -// } - std::optional Line::intersect(const Vector2 p1, const Vector2 p2, const Vector2 q1, const Vector2 q2) { Vector2 A = p1 - p2; Vector2 B = q1 - q2; From 7be4cfaa796e44d159edf195919e8388ad786e6f Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 15 Jan 2024 11:24:28 +0100 Subject: [PATCH 03/17] Fix avoidance of defense area in BallPlacementThem and BallPlacementUs Change dealer behaviour near target Change position of kicker during kick off prepare --- .../plays/referee_specific/BallPlacementThem.cpp | 3 +++ .../stp/plays/referee_specific/BallPlacementUs.cpp | 4 +++- .../plays/referee_specific/KickOffUsPrepare.cpp | 14 +------------- roboteam_ai/src/utilities/Dealer.cpp | 10 ++++++---- 4 files changed, 13 insertions(+), 18 deletions(-) diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp index 8052ce74b..b01d5c266 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp @@ -66,6 +66,9 @@ void BallPlacementThem::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); calculateInfoForHarasser(); + for (auto &stpInfo : stpInfos) { + stpInfo.second.setShouldAvoidDefenseArea(false); + } } void BallPlacementThem::calculateInfoForHarasser() noexcept { diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp index b0cbf8f37..4c011d08f 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp @@ -73,9 +73,11 @@ void BallPlacementUs::calculateInfoForRoles() noexcept { stpInfos["ball_placer"].setPositionToShootAt(ballTarget); stpInfos["ball_placer"].setPositionToMoveTo(ballTarget); - stpInfos["ball_placer"].setShouldAvoidDefenseArea(false); stpInfos["ball_placer"].setShouldAvoidOutOfField(false); stpInfos["ball_placer"].setShouldAvoidBall(false); + for (auto &stpInfo : stpInfos) { + stpInfo.second.setShouldAvoidDefenseArea(false); + } 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/KickOffUsPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp index d4dfa8d66..dcda17d67 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp @@ -69,19 +69,7 @@ void KickOffUsPrepare::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForFormationOurSide(stpInfos, roles, field, world); // The "kicker" will go to the ball - if (stpInfos["kicker"].getRobot() && stpInfos["kicker"].getRobot()->get()->getPos().x < 0) { - Vector2 robotPos = stpInfos["kicker"].getRobot()->get()->getPos(); - Vector2 ballPos = world->getWorld()->getBall()->get()->position; - if ((robotPos - ballPos).length() < 0.7) { - stpInfos["kicker"].setPositionToMoveTo(Vector2(-0.25, 0.0)); - } else if (robotPos.y > 0) { - stpInfos["kicker"].setPositionToMoveTo(Vector2(-0.25, 0.6)); - } else { - stpInfos["kicker"].setPositionToMoveTo(Vector2(-0.25, -0.6)); - } - } else { - stpInfos["kicker"].setPositionToMoveTo(Vector2(-0.25, 0.0)); - } + stpInfos["kicker"].setPositionToMoveTo(Vector2(-0.5, 0.0)); } const char* KickOffUsPrepare::getName() const { return "Kick Off Us Prepare"; } diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index 84aaa5769..367b0f106 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -261,10 +261,12 @@ 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; } - target_position = target_position.value() + stpInfo.getTargetLocationSpeed() * stp::control_constants::DEALER_SPEED_FACTOR; - - // Target found. Calculate distance - distance = (robot->getPos() + robot->getVel() * stp::control_constants::DEALER_SPEED_FACTOR).dist(*target_position); + double dealer_speed_factor = stp::control_constants::DEALER_SPEED_FACTOR; + if ((robot->getPos()-target_position.value()).length() < robot->getVel().length() * dealer_speed_factor) { + dealer_speed_factor = (robot->getPos()-target_position.value()).length() / robot->getVel().length(); + } + target_position = target_position.value() + stpInfo.getTargetLocationSpeed() * dealer_speed_factor; + distance = (robot->getPos() + robot->getVel() * dealer_speed_factor).dist(*target_position); return costForDistance(distance, field->playArea.width(), field->playArea.height()); } From b2c5841a9fe845409ab14ff9ef2e508ef13b3114 Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 15 Jan 2024 14:08:37 +0100 Subject: [PATCH 04/17] Update avoidShape during ball placement --- roboteam_ai/src/stp/computations/PositionComputations.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index ff6eb5949..8ec9f311b 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -178,7 +178,7 @@ Vector2 PositionComputations::calculateAvoidBallPosition(Vector2 targetPosition, // During ball placement, we need to avoid the area between the ball and the target position by a certain margin if (currentGameState == "ball_placement_us" || currentGameState == "ball_placement_them") { - avoidShape = std::make_unique(Tube(ballPosition, GameStateManager::getRefereeDesignatedPosition(), control_constants::AVOID_BALL_DISTANCE)); + avoidShape = std::make_unique(Tube(ballPosition, GameStateManager::getRefereeDesignatedPosition(), control_constants::AVOID_BALL_DISTANCE + 0.1)); } 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)); From c5fc9cb9a76377c296e426c78c0130a927e54a24 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 16 Jan 2024 10:27:04 +0100 Subject: [PATCH 05/17] Format --- .../include/roboteam_ai/stp/constants/ControlConstants.h | 3 ++- .../src/stp/plays/referee_specific/BallPlacementThem.cpp | 2 +- .../src/stp/plays/referee_specific/BallPlacementUs.cpp | 2 +- roboteam_ai/src/utilities/Dealer.cpp | 4 ++-- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index 06dd1dc6e..19f7664ca 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -75,7 +75,8 @@ constexpr double DISTANCE_FROM_GOAL_CLOSE = 2 * ROBOT_RADIUS; /**< Distance from /// GameState constants constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS; /**< Minimum distance all robots should keep when avoiding the ball */ -constexpr double MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT = AVOID_BALL_DISTANCE + 2 * ROBOT_RADIUS + 0.4; /**< Minimum distance between the defense area and the ball placement position to be able to pass */ +constexpr double MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT = + AVOID_BALL_DISTANCE + 2 * ROBOT_RADIUS + 0.4; /**< Minimum distance between the defense area and the ball placement position to be able to pass */ } // namespace rtt::ai::stp::control_constants diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp index b01d5c266..de6765dcd 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp @@ -66,7 +66,7 @@ void BallPlacementThem::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); calculateInfoForHarasser(); - for (auto &stpInfo : stpInfos) { + for (auto& stpInfo : stpInfos) { stpInfo.second.setShouldAvoidDefenseArea(false); } } diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp index 4c011d08f..49e86e8f4 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp @@ -75,7 +75,7 @@ void BallPlacementUs::calculateInfoForRoles() noexcept { stpInfos["ball_placer"].setPositionToMoveTo(ballTarget); stpInfos["ball_placer"].setShouldAvoidOutOfField(false); stpInfos["ball_placer"].setShouldAvoidBall(false); - for (auto &stpInfo : stpInfos) { + for (auto& stpInfo : stpInfos) { stpInfo.second.setShouldAvoidDefenseArea(false); } diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index 367b0f106..42ad33b5b 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -262,8 +262,8 @@ double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::Ro return 0; } double dealer_speed_factor = stp::control_constants::DEALER_SPEED_FACTOR; - if ((robot->getPos()-target_position.value()).length() < robot->getVel().length() * dealer_speed_factor) { - dealer_speed_factor = (robot->getPos()-target_position.value()).length() / robot->getVel().length(); + if ((robot->getPos() - target_position.value()).length() < robot->getVel().length() * dealer_speed_factor) { + dealer_speed_factor = (robot->getPos() - target_position.value()).length() / robot->getVel().length(); } target_position = target_position.value() + stpInfo.getTargetLocationSpeed() * dealer_speed_factor; distance = (robot->getPos() + robot->getVel() * dealer_speed_factor).dist(*target_position); From 36f9796bf2afd7ef89f3d3450bba518ccc43fa98 Mon Sep 17 00:00:00 2001 From: Jorn Date: Wed, 17 Jan 2024 10:40:57 +0100 Subject: [PATCH 06/17] Improve collision with robots and ball detection --- .../BBTrajectories/WorldObjects.cpp | 111 +++++++----------- .../src/stp/tactics/passive/BallStandBack.cpp | 4 +- .../include/roboteam_utils/LineSegment.h | 8 ++ roboteam_utils/src/utils/LineSegment.cpp | 12 ++ 4 files changed, 64 insertions(+), 71 deletions(-) diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 26376bcb3..11addee8a 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -94,7 +94,7 @@ void WorldObjects::calculateFieldCollisions(const rtt::Field &field, std::vector // Don't care about the field if the robot is already outside the field (i == 0 is the first point of the robot's path, so almost the currentPosition). if (i == 0) return; - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "FieldCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), "FieldCollision"}); return; } } @@ -111,7 +111,7 @@ void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std:: // Don't care about the defense area if the robot is already in the defense area. It should just get out as fast as possible :) // if (i == 0) return; - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "DefenseAreaCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), "DefenseAreaCollision"}); return; } } @@ -119,21 +119,21 @@ void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std:: void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, std::vector pathPoints, double timeStep, double dist, size_t completedTimeSteps) { + double ballAvoidanceTime = 0.5; + auto startPositionBall = world->getWorld()->getBall()->get()->position; auto VelocityBall = world->getWorld()->getBall()->get()->velocity; - std::vector ballTrajectory; - - double time = completedTimeSteps * timeStep; - double ballAvoidanceTime = 2; + Vector2 oldBallPos = startPositionBall; + Vector2 newBallPos = startPositionBall; - while (pathPoints.size() * timeStep > time && time < ballAvoidanceTime - completedTimeSteps * timeStep) { - ballTrajectory.emplace_back(startPositionBall + VelocityBall * time); - time += timeStep; - } - - for (size_t i = completedTimeSteps; i < ballTrajectory.size(); i++) { - if (pathPoints[i].dist(ballTrajectory[i]) < dist) { - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "BallCollision"}); + for (double loopTime = 0; loopTime < ballAvoidanceTime; loopTime += timeStep, completedTimeSteps++) { + oldBallPos = newBallPos; + newBallPos += VelocityBall * timeStep; + if (completedTimeSteps + 1 >= pathPoints.size()) { + return; + } + if (LineSegment(pathPoints[completedTimeSteps], pathPoints[completedTimeSteps + 1]).isWithinDistance(LineSegment(oldBallPos, newBallPos), dist)) { + insertCollisionData(collisionDatas, CollisionData{pathPoints[completedTimeSteps], loopTime, "BallCollision"}); return; } } @@ -142,66 +142,41 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, size_t completedTimeSteps) { const std::vector theirRobots = world->getWorld()->getThem(); - const double maxCollisionCheckTime = 1.0; // Maximum time to check for collisions - const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS_MAX(); // Distance to avoid enemy robots - - for (size_t i = completedTimeSteps + 1; i < pathPoints.size() - 1; i++) { - double currentTime = i * timeStep; - if (currentTime - completedTimeSteps * timeStep >= maxCollisionCheckTime) break; - - // Interpolate between the current point and the next point - Vector2 startPoint = pathPoints[i]; - Vector2 endPoint = pathPoints[i + 1]; - for (double t = 0; t <= 1; t += 0.3) { - Vector2 interpolatedPoint = startPoint * (1 - t) + endPoint * t; - double interpolatedTime = currentTime + t * timeStep; - - for (const auto &theirRobot : theirRobots) { - Vector2 theirVel = theirRobot->getVel(); - Vector2 theirPos = theirRobot->getPos() + theirVel * interpolatedTime; - Vector2 posDif = interpolatedPoint - theirPos; - - if (posDif.length() < avoidanceDistance) { - insertCollisionData(collisionDatas, CollisionData{interpolatedPoint, interpolatedTime, "EnemyRobotCollision"}); - return; - } + const double maxCollisionCheckTime = 1; // Maximum time to check for collisions + const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS_MAX() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; // Distance to avoid enemy robots + + for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { + if (completedTimeSteps + 1 >= pathPoints.size()) { + return; + } + for (const auto &theirRobot : theirRobots) { + if (LineSegment(pathPoints[completedTimeSteps], pathPoints[completedTimeSteps + 1]) + .isWithinDistance(LineSegment(theirRobot->getPos() + theirRobot->getVel() * loopTime, theirRobot->getPos() + theirRobot->getVel() * (loopTime + timeStep)), + avoidanceDistance)) { + insertCollisionData(collisionDatas, CollisionData{pathPoints[completedTimeSteps], loopTime, "TheirRobotCollision"}); + return; } } } } + void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, const std::unordered_map> &computedPaths, int robotId, double timeStep, size_t completedTimeSteps) { auto ourRobots = world->getWorld()->getUs(); - const double maxCollisionCheckTime = 1.0; // Maximum time to check for collisions - const double avoidanceDistance = 2.0 * ai::Constants::ROBOT_RADIUS(); // Distance to avoid our robots - // Because wallers are irritating and colide with each other, we ignore first 10 timesteps - // Otherwise, we get collisions in every direction and we just yoink through our own defense are - // TODO: Maybe fix this in a better way if we get a lot of collisions. - for (size_t i = completedTimeSteps + 6; i < pathPoints.size() - 1; i++) { - double currentTime = i * timeStep; - if (currentTime - completedTimeSteps * timeStep >= maxCollisionCheckTime) break; - - // Interpolate between the current point and the next point - Vector2 startPoint = pathPoints[i]; - Vector2 endPoint = pathPoints[i + 1]; - for (double t = 0; t <= 1; t += 0.3) { - Vector2 interpolatedPoint = startPoint * (1 - t) + endPoint * t; - double interpolatedTime = currentTime + t * timeStep; - - for (auto &robot : ourRobots) { - if (robotId != robot->getId()) { - Vector2 theirPos; - if (computedPaths.find(robot->getId()) != computedPaths.end() && computedPaths.at(robot->getId()).size() > (i + completedTimeSteps)) { - theirPos = computedPaths.at(robot->getId())[i + completedTimeSteps] * (1 - t) + computedPaths.at(robot->getId())[i + completedTimeSteps + 1] * t; - } else { - theirPos = robot->getPos(); - } - - if ((interpolatedPoint - theirPos).length() < avoidanceDistance) { - insertCollisionData(collisionDatas, CollisionData{interpolatedPoint, interpolatedTime, "OurRobotCollision"}); - return; - } - } + const double maxCollisionCheckTime = 1; // Maximum time to check for collisions + const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; // Distance to avoid our robots + + for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { + if (completedTimeSteps + 1 >= pathPoints.size()) { + return; + } + for (const auto &ourRobot : ourRobots) { + if (ourRobot->getId() == robotId) continue; + if (LineSegment(pathPoints[completedTimeSteps], pathPoints[completedTimeSteps + 1]) + .isWithinDistance(LineSegment(ourRobot->getPos() + ourRobot->getVel() * loopTime, ourRobot->getPos() + ourRobot->getVel() * (loopTime + timeStep)), + avoidanceDistance)) { + insertCollisionData(collisionDatas, CollisionData{pathPoints[completedTimeSteps], loopTime, "OurRobotCollision"}); + return; } } } @@ -217,7 +192,7 @@ void WorldObjects::calculateBallPlacementCollision(const rtt::world::World *worl for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { if (ballPlacementLine.distanceToLine(pathPoints[i]) < ai::stp::control_constants::AVOID_BALL_DISTANCE) { - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], i * timeStep, "BallPlacementCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), "BallPlacementCollision"}); return; } } diff --git a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp index 4579801bd..6ee894f27 100644 --- a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp @@ -23,13 +23,11 @@ std::optional BallStandBack::calculateInfoForSkill(StpInfo const &info) if (standStillCounter > 60) { auto moveVector = info.getRobot()->get()->getPos() - info.getBall()->get()->position; targetPos = info.getBall()->get()->position + moveVector.stretchToLength(0.80); - skillStpInfo.setShouldAvoidBall(true); } else { 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); diff --git a/roboteam_utils/include/roboteam_utils/LineSegment.h b/roboteam_utils/include/roboteam_utils/LineSegment.h index 08db2055e..9a5636303 100644 --- a/roboteam_utils/include/roboteam_utils/LineSegment.h +++ b/roboteam_utils/include/roboteam_utils/LineSegment.h @@ -175,6 +175,14 @@ class LineSegment { */ [[nodiscard]] std::optional getClosestPointToLine(const Line &) const; + /** + * Check whether two lineSegments are within a given distance of each other. + * @param line The other lineSegment. + * @param distance The distance => 0.0. + * @return True if the lineSegments are within the given distance of each other, false otherwise. + */ + [[nodiscard]] bool isWithinDistance(const LineSegment &line, double distance) const; + private: /** * Check whether a given point lies on this LineSegment, given that this LineSegment has length > 0 and that the given point lies on the infinite Line expansion of this diff --git a/roboteam_utils/src/utils/LineSegment.cpp b/roboteam_utils/src/utils/LineSegment.cpp index 9e6ea8ce8..9d657421f 100644 --- a/roboteam_utils/src/utils/LineSegment.cpp +++ b/roboteam_utils/src/utils/LineSegment.cpp @@ -266,4 +266,16 @@ std::optional LineSegment::getClosestPointToLine(const Line &other) con return std::nullopt; } +bool LineSegment::isWithinDistance(const LineSegment &line, double distance) const { + if (this->doesIntersect(line)) { + return true; + } + double dist1 = this->distanceToLine(line.start); + double dist2 = this->distanceToLine(line.end); + double dist3 = line.distanceToLine(this->start); + double dist4 = line.distanceToLine(this->end); + + return dist1 <= distance || dist2 <= distance || dist3 <= distance || dist4 <= distance; +} + } // namespace rtt From 253f7f94cf8be29baf38062e211ae479d92a09a0 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 19 Jan 2024 17:16:53 +0100 Subject: [PATCH 07/17] Refactor Robot and ball collision methods --- .../BBTrajectories/WorldObjects.h | 3 +- roboteam_ai/include/roboteam_ai/stp/StpInfo.h | 2 + .../roboteam_ai/utilities/StpInfoEnums.h | 3 +- .../BBTrajectories/WorldObjects.cpp | 128 +++++++++++++++--- .../stp/computations/PositionComputations.cpp | 24 ++-- .../include/roboteam_utils/LineSegment.h | 8 -- roboteam_utils/src/utils/LineSegment.cpp | 43 ++---- 7 files changed, 143 insertions(+), 68 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h index 91b788d7c..fe62d9d2e 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h @@ -121,9 +121,10 @@ class WorldObjects { * @param pathPoints std::vector with path points * @param timeStep Time between pathpoints * @param completedTimeSteps Amount of seconds of the trajectory already completed by the robot + * @param avoidId ID of the robot to ignore collisions with */ void calculateEnemyRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, - size_t completedTimeSteps); + size_t completedTimeSteps, int avoidId); /** * @brief Takes a path from the array of stored paths and checks points along the path if they are too close to diff --git a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h index bfa8db2a1..c23990d05 100644 --- a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h +++ b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h @@ -94,6 +94,8 @@ struct StpInfo { void setShouldAvoidTheirRobots(bool shouldAvoidTheirRobots) { avoidObjects.shouldAvoidTheirRobots = shouldAvoidTheirRobots; } + void setNotAvoidTheirRobotId(int notAvoidTheirRobotId) { avoidObjects.notAvoidTheirRobotId = notAvoidTheirRobotId; } + private: /** * Current world pointer diff --git a/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h b/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h index 5d063dbcf..338693e75 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h +++ b/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h @@ -38,11 +38,12 @@ enum class Status { Waiting, Success, Failure, Running }; */ struct AvoidObjects { bool shouldAvoidBall = false; /***< Indicates whether the robot should avoid the ball */ + double avoidBallDist = control_constants::AVOID_BALL_DISTANCE; /***< The minimum distance a robot should keep to the ball when avoiding the ball */ bool shouldAvoidDefenseArea = true; /***< Indicates whether the robot should avoid the defense area */ bool shouldAvoidOutOfField = true; /***< Indicates whether the robot should avoid going out of the field */ bool shouldAvoidOurRobots = true; /***< Indicates whether the robot should avoid allied robots */ bool shouldAvoidTheirRobots = true; /***< Indicates whether the robot should avoid the enemy robots */ - double avoidBallDist = control_constants::AVOID_BALL_DISTANCE; /***< The minimum distance a robot should keep to the ball when avoiding the ball */ + int notAvoidTheirRobotId = -1; /***< Indicates which robot of the enemy team should not be avoided, -1 means all robots should be avoided */ }; } // namespace rtt::ai::stp #endif // RTT_STPINFOENUMS_H diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 11addee8a..354c0999e 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -42,8 +42,8 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W if (avoidObjects.shouldAvoidBall) { calculateBallCollisions(world, collisionDatas, pathPoints, timeStep, avoidObjects.avoidBallDist, completedTimeSteps); } - if (avoidObjects.shouldAvoidTheirRobots) { - calculateEnemyRobotCollisions(world, collisionDatas, pathPoints, timeStep, completedTimeSteps); + if (avoidObjects.shouldAvoidTheirRobots || avoidObjects.notAvoidTheirRobotId != -1) { + calculateEnemyRobotCollisions(world, collisionDatas, pathPoints, timeStep, completedTimeSteps, avoidObjects.notAvoidTheirRobotId); } if (avoidObjects.shouldAvoidOurRobots) { calculateOurRobotCollisions(world, collisionDatas, pathPoints, computedPaths, robotId, timeStep, completedTimeSteps); @@ -123,16 +123,43 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: auto startPositionBall = world->getWorld()->getBall()->get()->position; auto VelocityBall = world->getWorld()->getBall()->get()->velocity; - Vector2 oldBallPos = startPositionBall; - Vector2 newBallPos = startPositionBall; for (double loopTime = 0; loopTime < ballAvoidanceTime; loopTime += timeStep, completedTimeSteps++) { - oldBallPos = newBallPos; - newBallPos += VelocityBall * timeStep; if (completedTimeSteps + 1 >= pathPoints.size()) { return; } - if (LineSegment(pathPoints[completedTimeSteps], pathPoints[completedTimeSteps + 1]).isWithinDistance(LineSegment(oldBallPos, newBallPos), dist)) { + if ((pathPoints[completedTimeSteps] - (startPositionBall + VelocityBall * loopTime)).length() > 1) { + continue; + } + double startPointX_R1 = pathPoints[completedTimeSteps].x; + double startPointY_R1 = pathPoints[completedTimeSteps].y; + double startPointX_ball = startPositionBall.x; + double startPointY_ball = startPositionBall.y; + + double velocityX_R1 = (pathPoints[completedTimeSteps + 1].x - startPointX_R1) / timeStep; + double velocityY_R1 = (pathPoints[completedTimeSteps + 1].y - startPointY_R1) / timeStep; + double velocityX_ball = VelocityBall.x; + double velocityY_ball = VelocityBall.y; + + double velocityX_diff = velocityX_R1 - velocityX_ball; + double velocityY_diff = velocityY_R1 - velocityY_ball; + + double minTime = (startPointX_R1 * velocityX_diff + velocityX_R1 * startPointX_ball - startPointX_ball * velocityX_ball - startPointY_R1 * velocityY_R1 + + startPointY_R1 * velocityY_ball + velocityY_R1 * startPointY_ball - startPointY_ball * velocityY_ball) / + (velocityX_R1 * velocityX_R1 - 2 * velocityX_R1 * velocityX_ball + velocityX_ball * velocityX_ball + velocityY_diff * velocityY_diff); + if (minTime < 0) { + minTime = 0; + } + if (minTime > timeStep) { + minTime = timeStep; + } + + // check distance at minTime + Vector2 ourLocationAtMinTime = Vector2(startPointX_R1, startPointY_R1) + Vector2(velocityX_R1, velocityY_R1) * minTime; + Vector2 theirLocationAtMinTime = Vector2(startPointX_ball, startPointY_ball) + Vector2(velocityX_ball, velocityY_ball) * minTime; + double distance = (ourLocationAtMinTime - theirLocationAtMinTime).length(); + + if (distance < dist) { insertCollisionData(collisionDatas, CollisionData{pathPoints[completedTimeSteps], loopTime, "BallCollision"}); return; } @@ -140,20 +167,53 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: } void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, - double timeStep, size_t completedTimeSteps) { + double timeStep, size_t completedTimeSteps, int avoidId) { const std::vector theirRobots = world->getWorld()->getThem(); - const double maxCollisionCheckTime = 1; // Maximum time to check for collisions + const double maxCollisionCheckTime = 0.5; // Maximum time to check for collisions const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS_MAX() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; // Distance to avoid enemy robots for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { if (completedTimeSteps + 1 >= pathPoints.size()) { return; } - for (const auto &theirRobot : theirRobots) { - if (LineSegment(pathPoints[completedTimeSteps], pathPoints[completedTimeSteps + 1]) - .isWithinDistance(LineSegment(theirRobot->getPos() + theirRobot->getVel() * loopTime, theirRobot->getPos() + theirRobot->getVel() * (loopTime + timeStep)), - avoidanceDistance)) { - insertCollisionData(collisionDatas, CollisionData{pathPoints[completedTimeSteps], loopTime, "TheirRobotCollision"}); + for (const auto &opponentRobot : theirRobots) { + if ((pathPoints[completedTimeSteps] + (pathPoints[completedTimeSteps + 1] - pathPoints[completedTimeSteps]) / timeStep * loopTime - + (opponentRobot->getPos() + opponentRobot->getVel() * loopTime)) + .length() > 1) { + continue; + } + + double startPointX_OurRobot = pathPoints[completedTimeSteps].x; + double startPointY_OurRobot = pathPoints[completedTimeSteps].y; + double startPointX_OpponentRobot = opponentRobot->getPos().x + opponentRobot->getVel().x * loopTime; + double startPointY_OpponentRobot = opponentRobot->getPos().y + opponentRobot->getVel().y * loopTime; + + double velocityX_OurRobot = (pathPoints[completedTimeSteps + 1].x - startPointX_OurRobot) / timeStep; + double velocityY_OurRobot = (pathPoints[completedTimeSteps + 1].y - startPointY_OurRobot) / timeStep; + double velocityX_OpponentRobot = opponentRobot->getVel().x; + double velocityY_OpponentRobot = opponentRobot->getVel().y; + + double velocityX_diff = velocityX_OurRobot - velocityX_OpponentRobot; + double velocityY_diff = velocityY_OurRobot - velocityY_OpponentRobot; + + double minTime = (startPointX_OurRobot * velocityX_diff + velocityX_OurRobot * startPointX_OpponentRobot - startPointX_OpponentRobot * velocityX_OpponentRobot - + startPointY_OurRobot * velocityY_OurRobot + startPointY_OurRobot * velocityY_OpponentRobot + velocityY_OurRobot * startPointY_OpponentRobot - + startPointY_OpponentRobot * velocityY_OpponentRobot) / + (velocityX_OurRobot * velocityX_OurRobot - 2 * velocityX_OurRobot * velocityX_OpponentRobot + velocityX_OpponentRobot * velocityX_OpponentRobot + + velocityY_diff * velocityY_diff); + if (minTime < 0) { + minTime = 0; + } + if (minTime > timeStep) { + minTime = timeStep; + } + + // check distance at minTime + Vector2 ourLocationAtMinTime = Vector2(startPointX_OurRobot, startPointY_OurRobot) + Vector2(velocityX_OurRobot, velocityY_OurRobot) * minTime; + Vector2 theirLocationAtMinTime = Vector2(startPointX_OpponentRobot, startPointY_OpponentRobot) + Vector2(velocityX_OpponentRobot, velocityY_OpponentRobot) * minTime; + double distance = (ourLocationAtMinTime - theirLocationAtMinTime).length(); + if (distance < avoidanceDistance) { + insertCollisionData(collisionDatas, CollisionData{ourLocationAtMinTime, loopTime, "TheirRobotCollision"}); return; } } @@ -163,7 +223,7 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, const std::unordered_map> &computedPaths, int robotId, double timeStep, size_t completedTimeSteps) { auto ourRobots = world->getWorld()->getUs(); - const double maxCollisionCheckTime = 1; // Maximum time to check for collisions + const double maxCollisionCheckTime = 0.5; // Maximum time to check for collisions const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; // Distance to avoid our robots for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { @@ -172,10 +232,40 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s } for (const auto &ourRobot : ourRobots) { if (ourRobot->getId() == robotId) continue; - if (LineSegment(pathPoints[completedTimeSteps], pathPoints[completedTimeSteps + 1]) - .isWithinDistance(LineSegment(ourRobot->getPos() + ourRobot->getVel() * loopTime, ourRobot->getPos() + ourRobot->getVel() * (loopTime + timeStep)), - avoidanceDistance)) { - insertCollisionData(collisionDatas, CollisionData{pathPoints[completedTimeSteps], loopTime, "OurRobotCollision"}); + if ((pathPoints[completedTimeSteps] - (ourRobot->getPos() + ourRobot->getVel() * loopTime)).length() > 1) { + continue; + } + + double startPointX_OurRobot = pathPoints[completedTimeSteps].x; + double startPointY_OurRobot = pathPoints[completedTimeSteps].y; + double startPointX_OtherRobot = ourRobot->getPos().x + ourRobot->getVel().x * loopTime; + double startPointY_OtherRobot = ourRobot->getPos().y + ourRobot->getVel().y * loopTime; + + double velocityX_OurRobot = (pathPoints[completedTimeSteps + 1].x - startPointX_OurRobot) / timeStep; + double velocityY_OurRobot = (pathPoints[completedTimeSteps + 1].y - startPointY_OurRobot) / timeStep; + double velocityX_OtherRobot = ourRobot->getVel().x; + double velocityY_OtherRobot = ourRobot->getVel().y; + + double velocityX_diff = velocityX_OurRobot - velocityX_OtherRobot; + double velocityY_diff = velocityY_OurRobot - velocityY_OtherRobot; + + double minTime = (startPointX_OurRobot * velocityX_diff + velocityX_OurRobot * startPointX_OtherRobot - startPointX_OtherRobot * velocityX_OtherRobot - + startPointY_OurRobot * velocityY_OurRobot + startPointY_OurRobot * velocityY_OtherRobot + velocityY_OurRobot * startPointY_OtherRobot - + startPointY_OtherRobot * velocityY_OtherRobot) / + (velocityX_OurRobot * velocityX_OurRobot - 2 * velocityX_OurRobot * velocityX_OtherRobot + velocityX_OtherRobot * velocityX_OtherRobot + + velocityY_diff * velocityY_diff); + if (minTime < 0) { + minTime = 0; + } + if (minTime > timeStep) { + minTime = timeStep; + } + + Vector2 ourLocationAtMinTime = Vector2(startPointX_OurRobot, startPointY_OurRobot) + Vector2(velocityX_OurRobot, velocityY_OurRobot) * minTime; + Vector2 otherRobotLocationAtMinTime = Vector2(startPointX_OtherRobot, startPointY_OtherRobot) + Vector2(velocityX_OtherRobot, velocityY_OtherRobot) * minTime; + double distance = (ourLocationAtMinTime - otherRobotLocationAtMinTime).length(); + if (distance < avoidanceDistance) { + insertCollisionData(collisionDatas, CollisionData{ourLocationAtMinTime, loopTime, "OurRobotCollision"}); return; } } diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 8ec9f311b..374151054 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -249,8 +249,12 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_mapget()->getPos().dist(field.leftGoalArea.rightLine().center()) > + stpInfos["harasser"].getRobot()->get()->getPos().dist(field.leftGoalArea.rightLine().center())) { + stpInfos["harasser"].setNotAvoidTheirRobotId(enemyClosestToBall->get()->getId()); + } else { + stpInfos["harasser"].setNotAvoidTheirRobotId(-1); + } auto harasser = std::find_if(roles->begin(), roles->end(), [](const std::unique_ptr &role) { return role != nullptr && role->getName() == "harasser"; }); if (harasser != roles->end() && !harasser->get()->finished() && strcmp(harasser->get()->getCurrentTactic()->getName(), "Formation") == 0) harasser->get()->forceNextTactic(); @@ -281,6 +285,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma erase_if(enemyRobots, [&](const auto enemyRobot) -> bool { return enemyClosestToBall && enemyRobot->getId() == enemyClosestToBall.value()->getId(); }); std::map enemyMap; std::vector enemies; + std::vector ids; for (auto enemy : enemyRobots) { if (enemy->hasBall()) continue; double score = FieldComputations::getDistanceToGoal(field, true, enemy->getPos()); @@ -290,6 +295,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma } EnemyInfo info = {enemy->getPos(), enemy->getVel(), enemy->getId()}; enemyMap.insert({score, info}); + ids.emplace_back(enemy->getId()); } ComputationManager::calculatedEnemyMapIds.clear(); // If defenders do not have a position yet, don't do hungarian algorithm @@ -343,11 +349,14 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma } else { stpInfos[activeDefenderNames[i]].setPositionToDefend(enemies[assignments[i]]); stpInfos[activeDefenderNames[i]].setBlockDistance(BlockDistance::ROBOTRADIUS); - constexpr double IGNORE_COLLISIONS_DISTANCE = 1.5; + constexpr double IGNORE_COLLISIONS_DISTANCE = 0.4; if (stpInfos[activeDefenderNames[i]].getRobot() && - (stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos() - enemies[assignments[i]]).length() < IGNORE_COLLISIONS_DISTANCE) { - stpInfos[activeDefenderNames[i]].setShouldAvoidOurRobots(false); - stpInfos[activeDefenderNames[i]].setShouldAvoidTheirRobots(false); + (stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos() - enemies[assignments[i]]).length() < IGNORE_COLLISIONS_DISTANCE && + stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos().dist(field.leftGoalArea.rightLine().center()) < + enemies[assignments[i]].dist(field.leftGoalArea.rightLine().center())) { + stpInfos[activeDefenderNames[i]].setNotAvoidTheirRobotId(ids[assignments[i]]); + } else { + stpInfos[activeDefenderNames[i]].setNotAvoidTheirRobotId(-1); } } } @@ -369,9 +378,8 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma wallerStpInfo.setAngle((world->getWorld()->getBall()->get()->position - field.leftGoalArea.rightLine().center()).angle()); // If the waller is close to its target, ignore collisions - constexpr double IGNORE_COLLISIONS_DISTANCE = 1.0; + constexpr double IGNORE_COLLISIONS_DISTANCE = 0.4; if (wallerStpInfo.getRobot() && (wallerStpInfo.getRobot()->get()->getPos() - positionToMoveTo).length() < IGNORE_COLLISIONS_DISTANCE) { - wallerStpInfo.setShouldAvoidOurRobots(false); wallerStpInfo.setShouldAvoidTheirRobots(false); } } diff --git a/roboteam_utils/include/roboteam_utils/LineSegment.h b/roboteam_utils/include/roboteam_utils/LineSegment.h index 9a5636303..08db2055e 100644 --- a/roboteam_utils/include/roboteam_utils/LineSegment.h +++ b/roboteam_utils/include/roboteam_utils/LineSegment.h @@ -175,14 +175,6 @@ class LineSegment { */ [[nodiscard]] std::optional getClosestPointToLine(const Line &) const; - /** - * Check whether two lineSegments are within a given distance of each other. - * @param line The other lineSegment. - * @param distance The distance => 0.0. - * @return True if the lineSegments are within the given distance of each other, false otherwise. - */ - [[nodiscard]] bool isWithinDistance(const LineSegment &line, double distance) const; - private: /** * Check whether a given point lies on this LineSegment, given that this LineSegment has length > 0 and that the given point lies on the infinite Line expansion of this diff --git a/roboteam_utils/src/utils/LineSegment.cpp b/roboteam_utils/src/utils/LineSegment.cpp index 9d657421f..d2658ff29 100644 --- a/roboteam_utils/src/utils/LineSegment.cpp +++ b/roboteam_utils/src/utils/LineSegment.cpp @@ -24,37 +24,31 @@ bool LineSegment::doesIntersect(const LineSegment &line) const { return intersec std::optional LineSegment::intersects(const LineSegment &line) const { auto result = Line::intersect(start, end, line.start, line.end); if (result.has_value()) { - float t = Line::relativePosition(start, end, result.value()); - float u = Line::relativePosition(line.start, line.end, result.value()); + Vector2 intersectionPoint = result.value(); + float t = Line::relativePosition(start, end, intersectionPoint); + float u = Line::relativePosition(line.start, line.end, intersectionPoint); if (t >= 0 && t <= 1 && u >= 0 && u <= 1) { return result; - } else { - return std::nullopt; } } else { - /* The only possible cases are that one/both of the LineSegments are actually points, the LineSegments have a shared LineSegment part or the LineSegments are distinct and - * parallel. */ if (isPoint()) { return line.isOnLine(start) ? std::optional(start) : std::nullopt; } else if (line.isPoint()) { return isOnLine(line.start) ? std::optional(line.start) : std::nullopt; } - // Check if both LineSegments are on the same infinite line. Line checkLine = Line(*this); - if (!checkLine.isOnLine(line.start)) { - return std::nullopt; - } - if (line.isOnFiniteLine(start)) { - return start; - } else if (line.isOnFiniteLine(end)) { - return end; - } else if (isOnFiniteLine(line.start)) { - return line.start; - } else { - return std::nullopt; + if (checkLine.isOnLine(line.start)) { + if (line.isOnFiniteLine(start)) { + return start; + } else if (line.isOnFiniteLine(end)) { + return end; + } else if (isOnFiniteLine(line.start)) { + return line.start; + } } } + return std::nullopt; } double LineSegment::distanceToLine(const Vector2 &point) const { return (project(point) - point).length(); } @@ -265,17 +259,4 @@ std::optional LineSegment::getClosestPointToLine(const Line &other) con return std::nullopt; } - -bool LineSegment::isWithinDistance(const LineSegment &line, double distance) const { - if (this->doesIntersect(line)) { - return true; - } - double dist1 = this->distanceToLine(line.start); - double dist2 = this->distanceToLine(line.end); - double dist3 = line.distanceToLine(this->start); - double dist4 = line.distanceToLine(this->end); - - return dist1 <= distance || dist2 <= distance || dist3 <= distance || dist4 <= distance; -} - } // namespace rtt From a28f1daa666a9002da82340eefdedb508cb58af5 Mon Sep 17 00:00:00 2001 From: Jorn Date: Sat, 20 Jan 2024 22:10:46 +0100 Subject: [PATCH 08/17] - Add completedTimeSteps to PositionControl class - Change OurRobotCollission check based on completedTimeSteps - Move scoring of paths to separate scoring function --- .../BBTrajectories/WorldObjects.h | 12 +- .../control/positionControl/PositionControl.h | 24 +- .../stp/constants/ControlConstants.h | 2 - .../BBTrajectories/WorldObjects.cpp | 214 +++++++-------- .../positionControl/PositionControl.cpp | 259 +++++++----------- roboteam_ai/src/stp/skills/GoToPos.cpp | 4 - 6 files changed, 224 insertions(+), 291 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h index fe62d9d2e..1d826043e 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h @@ -57,13 +57,16 @@ class WorldObjects { * @param computedPaths the paths of our robots * @param robotId Id of the robot * @param avoidObjects a struct with if it should avoid certain objects + * @param completedTimeSteps Number of completed time steps * @return optional with rtt::BB::CollisionData */ std::optional getFirstCollision(const rtt::world::World *world, const rtt::Field &field, const rtt::Trajectory2D &Trajectory, - const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects); + const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects, + std::unordered_map completedTimeSteps); /** - * @brief Calculates the position of the first collision with the defense area + * @brief Calculates the position of the first collision with the defense area, note that this functions assumes the entire trajectory, + * including the part that might already be completed. Only call this function for a new trajectory * @param field used for checking collisions with the field * @param BBTrajectory the trajectory to check for collisions * @param computedPaths the paths of our robots @@ -109,7 +112,7 @@ class WorldObjects { * @param dist Distance to the ball * @param completedTimeSteps Number of completed time steps */ - void calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, std::vector pathPoints, double timeStep, double dist, + void calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, double dist, size_t completedTimeSteps); /** @@ -139,7 +142,8 @@ class WorldObjects { * @param completedTimeSteps how many seconds of the trajectory is already completed by the robot */ void calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, - const std::unordered_map> &computedPaths, int robotId, double timeStep, size_t completedTimeSteps); + const std::unordered_map> &computedPaths, int robotId, double timeStep, + std::unordered_map completedTimeSteps); /** * @brief Takes a calculated path of a robot and checks points along the path if they are too close to an diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index 038d28b18..dcba2eb55 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -34,6 +34,7 @@ class PositionControl { std::unordered_map> computedPaths; /**< Map of computed paths for each robot */ std::unordered_map> computedPathsVel; /**< Map of computed velocities for each robot */ std::unordered_map>> computedPathsPosVel; /**< Map of pairs containing position and velocity for each robot */ + std::unordered_map completedTimeSteps; /**< Map of completed time steps for each robot */ /** * @brief Checks if the current path should be recalculated: @@ -104,10 +105,23 @@ 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 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 + * @param world a pointer to the current world + * @param field the field object, used onwards by the collision detector + * @param robotId the ID of the robot for which the path is calculated + * @param firstCollision location of the first collision on the current path + * @param trajectoryAroundCollision the trajectory to the intermediate point + * @return A score for the trajectory + */ + double calculateScore(const rtt::world::World *world, const rtt::Field &field, int robotId, std::optional &firstCollision, + Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects); + /** * @brief Tries to find a new trajectory when the current path has a collision on it. It tries this by * looking for trajectories which go to intermediate points in the area of the collision and from these - * paths again to the target. Also draws the intermediate point and path in the interface + * paths again to the target * @param world the world object * @param field the field object, used onwards by the collision detector * @param robotId the ID of the robot for which the path is calculated @@ -140,16 +154,14 @@ class PositionControl { Vector2 &targetPosition, int robotId, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects); /** - * @brief Creates intermediate points to make a path to. First, a pointToDrawFrom is picked by drawing a line - * from the target position to the obstacle and extending that line further towards our currentPosition. - * Second, make half circle of intermediatePoints pointed towards obstaclePosition, originating from pointToDrawFrom + * @brief Creates intermediate points to make a path to. These points all have equal distance to the + * collision point * @param field the field object, used onwards by the collision detector - * @param robotId the ID of the robot for which the path is calculated * @param firstCollision location of the first collision on the current path * @param targetPosition the desired position that the robot has to reach * @return A vector with coordinates of the intermediate points */ - std::vector createIntermediatePoints(const rtt::Field &field, int robotId, std::optional &firstCollision, Vector2 &targetPosition); + std::vector createIntermediatePoints(const rtt::Field &field, std::optional &firstCollision, Vector2 &targetPosition); }; } // namespace rtt::ai::control diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index 19f7664ca..f8efbd460 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -75,8 +75,6 @@ constexpr double DISTANCE_FROM_GOAL_CLOSE = 2 * ROBOT_RADIUS; /**< Distance from /// GameState constants constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS; /**< Minimum distance all robots should keep when avoiding the ball */ -constexpr double MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT = - AVOID_BALL_DISTANCE + 2 * ROBOT_RADIUS + 0.4; /**< Minimum distance between the defense area and the ball placement position to be able to pass */ } // namespace rtt::ai::stp::control_constants diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 354c0999e..76ba3a2bc 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -14,42 +14,30 @@ namespace rtt::BB { WorldObjects::WorldObjects() = default; std::optional WorldObjects::getFirstCollision(const rtt::world::World *world, const rtt::Field &field, const Trajectory2D &Trajectory, - const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects) { - // Set the time step for the trajectory calculation + const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects, + std::unordered_map completedTimeSteps) { double timeStep = 0.1; - - // Calculate the points on the trajectory at intervals of `timeStep` auto pathPoints = Trajectory.getPathApproach(timeStep); - // If the computed path for the robot exists and has more than 10 points, - // and `pathPoints` also has more than 10 points, remove the last 3 points from `pathPoints`. - // This avoids checking for collisions at the endpoint of the trajectory. - if (computedPaths.contains(robotId) && computedPaths.at(robotId).size() > 10 && pathPoints.size() > 10) { - pathPoints.erase(pathPoints.end() - 3, pathPoints.end()); - } - - // Calculate the number of time steps that have been completed - size_t completedTimeSteps = computedPaths.contains(robotId) && pathPoints.size() > computedPaths.at(robotId).size() ? pathPoints.size() - computedPaths.at(robotId).size() : 0; - std::vector collisionDatas; if (avoidObjects.shouldAvoidOutOfField) { - calculateFieldCollisions(field, collisionDatas, pathPoints, timeStep, completedTimeSteps); + calculateFieldCollisions(field, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); } if (avoidObjects.shouldAvoidDefenseArea) { - calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep, completedTimeSteps); + calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep, completedTimeSteps[robotId]); } if (avoidObjects.shouldAvoidBall) { - calculateBallCollisions(world, collisionDatas, pathPoints, timeStep, avoidObjects.avoidBallDist, completedTimeSteps); + calculateBallCollisions(world, collisionDatas, pathPoints, timeStep, avoidObjects.avoidBallDist, completedTimeSteps[robotId]); } if (avoidObjects.shouldAvoidTheirRobots || avoidObjects.notAvoidTheirRobotId != -1) { - calculateEnemyRobotCollisions(world, collisionDatas, pathPoints, timeStep, completedTimeSteps, avoidObjects.notAvoidTheirRobotId); + calculateEnemyRobotCollisions(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId], avoidObjects.notAvoidTheirRobotId); } if (avoidObjects.shouldAvoidOurRobots) { calculateOurRobotCollisions(world, collisionDatas, pathPoints, computedPaths, robotId, timeStep, completedTimeSteps); } if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { - calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps); + calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); } if (!collisionDatas.empty()) { return collisionDatas[0]; @@ -60,25 +48,11 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W std::optional WorldObjects::getFirstDefenseAreaCollision(const rtt::Field &field, const Trajectory2D &Trajectory, const std::unordered_map> &computedPaths, int robotId) { - // Set the time step for the trajectory calculation double timeStep = 0.1; - - // Calculate the points on the trajectory at intervals of `timeStep` auto pathPoints = Trajectory.getPathApproach(timeStep); - // If the computed path for the robot exists and has more than 10 points, - // and `pathPoints` also has more than 10 points, remove the last 3 points from `pathPoints`. - // This avoids checking for collisions at the endpoint of the trajectory. - if (computedPaths.contains(robotId) && computedPaths.at(robotId).size() > 10 && pathPoints.size() > 10) { - pathPoints.erase(pathPoints.end() - 3, pathPoints.end()); - } - - // Calculate the number of time steps that have been completed - size_t completedTimeSteps = computedPaths.contains(robotId) && pathPoints.size() > computedPaths.at(robotId).size() ? pathPoints.size() - computedPaths.at(robotId).size() : 0; - std::vector collisionDatas; - - calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep, completedTimeSteps); + calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep, 0); if (!collisionDatas.empty()) { return collisionDatas[0]; @@ -89,8 +63,9 @@ std::optional WorldObjects::getFirstDefenseAreaCollision(const rt void WorldObjects::calculateFieldCollisions(const rtt::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, size_t completedTimeSteps) { + double robotRadius = rtt::ai::Constants::ROBOT_RADIUS(); for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { - if (!field.playArea.contains(pathPoints[i], rtt::ai::Constants::ROBOT_RADIUS())) { + if (!field.playArea.contains(pathPoints[i], robotRadius)) { // Don't care about the field if the robot is already outside the field (i == 0 is the first point of the robot's path, so almost the currentPosition). if (i == 0) return; @@ -103,8 +78,8 @@ void WorldObjects::calculateFieldCollisions(const rtt::Field &field, std::vector void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, int robotId, double timeStep, size_t completedTimeSteps) { auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = rtt::ai::FieldComputations::getDefenseAreaMargin(); - auto ourDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1); - auto theirDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, false, theirDefenseAreaMargin, 1); + const auto &ourDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1); + const auto &theirDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, false, theirDefenseAreaMargin, 1); for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { if (ourDefenseArea.contains(pathPoints[i]) || theirDefenseArea.contains(pathPoints[i])) { @@ -117,12 +92,12 @@ void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std:: } } -void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, std::vector pathPoints, double timeStep, +void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, double dist, size_t completedTimeSteps) { - double ballAvoidanceTime = 0.5; + double ballAvoidanceTime = 0.7; - auto startPositionBall = world->getWorld()->getBall()->get()->position; - auto VelocityBall = world->getWorld()->getBall()->get()->velocity; + const Vector2 &startPositionBall = world->getWorld()->getBall()->get()->position; + const Vector2 &VelocityBall = world->getWorld()->getBall()->get()->velocity; for (double loopTime = 0; loopTime < ballAvoidanceTime; loopTime += timeStep, completedTimeSteps++) { if (completedTimeSteps + 1 >= pathPoints.size()) { @@ -131,30 +106,26 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: if ((pathPoints[completedTimeSteps] - (startPositionBall + VelocityBall * loopTime)).length() > 1) { continue; } - double startPointX_R1 = pathPoints[completedTimeSteps].x; - double startPointY_R1 = pathPoints[completedTimeSteps].y; - double startPointX_ball = startPositionBall.x; - double startPointY_ball = startPositionBall.y; + const Vector2 ¤tPoint = pathPoints[completedTimeSteps]; + const Vector2 &nextPoint = pathPoints[completedTimeSteps + 1]; + const double startPointX_R1 = currentPoint.x; + const double startPointY_R1 = currentPoint.y; + const double startPointX_ball = startPositionBall.x; + const double startPointY_ball = startPositionBall.y; - double velocityX_R1 = (pathPoints[completedTimeSteps + 1].x - startPointX_R1) / timeStep; - double velocityY_R1 = (pathPoints[completedTimeSteps + 1].y - startPointY_R1) / timeStep; - double velocityX_ball = VelocityBall.x; - double velocityY_ball = VelocityBall.y; + const double velocityX_R1 = (nextPoint.x - startPointX_R1) / timeStep; + const double velocityY_R1 = (nextPoint.y - startPointY_R1) / timeStep; + const double velocityX_ball = VelocityBall.x; + const double velocityY_ball = VelocityBall.y; - double velocityX_diff = velocityX_R1 - velocityX_ball; - double velocityY_diff = velocityY_R1 - velocityY_ball; + const double velocityX_diff = velocityX_R1 - velocityX_ball; + const double velocityY_diff = velocityY_R1 - velocityY_ball; double minTime = (startPointX_R1 * velocityX_diff + velocityX_R1 * startPointX_ball - startPointX_ball * velocityX_ball - startPointY_R1 * velocityY_R1 + startPointY_R1 * velocityY_ball + velocityY_R1 * startPointY_ball - startPointY_ball * velocityY_ball) / (velocityX_R1 * velocityX_R1 - 2 * velocityX_R1 * velocityX_ball + velocityX_ball * velocityX_ball + velocityY_diff * velocityY_diff); - if (minTime < 0) { - minTime = 0; - } - if (minTime > timeStep) { - minTime = timeStep; - } + minTime = std::clamp(minTime, 0.0, timeStep); - // check distance at minTime Vector2 ourLocationAtMinTime = Vector2(startPointX_R1, startPointY_R1) + Vector2(velocityX_R1, velocityY_R1) * minTime; Vector2 theirLocationAtMinTime = Vector2(startPointX_ball, startPointY_ball) + Vector2(velocityX_ball, velocityY_ball) * minTime; double distance = (ourLocationAtMinTime - theirLocationAtMinTime).length(); @@ -168,50 +139,42 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, size_t completedTimeSteps, int avoidId) { - const std::vector theirRobots = world->getWorld()->getThem(); - const double maxCollisionCheckTime = 0.5; // Maximum time to check for collisions - const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS_MAX() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; // Distance to avoid enemy robots + const std::vector &theirRobots = world->getWorld()->getThem(); + const double maxCollisionCheckTime = 0.7; + const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS_MAX() + ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { if (completedTimeSteps + 1 >= pathPoints.size()) { return; } - for (const auto &opponentRobot : theirRobots) { - if ((pathPoints[completedTimeSteps] + (pathPoints[completedTimeSteps + 1] - pathPoints[completedTimeSteps]) / timeStep * loopTime - - (opponentRobot->getPos() + opponentRobot->getVel() * loopTime)) - .length() > 1) { - continue; - } - double startPointX_OurRobot = pathPoints[completedTimeSteps].x; - double startPointY_OurRobot = pathPoints[completedTimeSteps].y; - double startPointX_OpponentRobot = opponentRobot->getPos().x + opponentRobot->getVel().x * loopTime; - double startPointY_OpponentRobot = opponentRobot->getPos().y + opponentRobot->getVel().y * loopTime; + const Vector2 ¤tPoint = pathPoints[completedTimeSteps]; + const Vector2 &nextPoint = pathPoints[completedTimeSteps + 1]; + const double startPointX_OurRobot = currentPoint.x; + const double startPointY_OurRobot = currentPoint.y; + const double velocityX_OurRobot = (nextPoint.x - startPointX_OurRobot) / timeStep; + const double velocityY_OurRobot = (nextPoint.y - startPointY_OurRobot) / timeStep; - double velocityX_OurRobot = (pathPoints[completedTimeSteps + 1].x - startPointX_OurRobot) / timeStep; - double velocityY_OurRobot = (pathPoints[completedTimeSteps + 1].y - startPointY_OurRobot) / timeStep; - double velocityX_OpponentRobot = opponentRobot->getVel().x; - double velocityY_OpponentRobot = opponentRobot->getVel().y; + for (const auto &opponentRobot : theirRobots) { + const double startPointX_OpponentRobot = opponentRobot->getPos().x + opponentRobot->getVel().x * loopTime; + const double startPointY_OpponentRobot = opponentRobot->getPos().y + opponentRobot->getVel().y * loopTime; + const double velocityX_OpponentRobot = opponentRobot->getVel().x; + const double velocityY_OpponentRobot = opponentRobot->getVel().y; - double velocityX_diff = velocityX_OurRobot - velocityX_OpponentRobot; - double velocityY_diff = velocityY_OurRobot - velocityY_OpponentRobot; + const double velocityX_diff = velocityX_OurRobot - velocityX_OpponentRobot; + const double velocityY_diff = velocityY_OurRobot - velocityY_OpponentRobot; double minTime = (startPointX_OurRobot * velocityX_diff + velocityX_OurRobot * startPointX_OpponentRobot - startPointX_OpponentRobot * velocityX_OpponentRobot - startPointY_OurRobot * velocityY_OurRobot + startPointY_OurRobot * velocityY_OpponentRobot + velocityY_OurRobot * startPointY_OpponentRobot - startPointY_OpponentRobot * velocityY_OpponentRobot) / (velocityX_OurRobot * velocityX_OurRobot - 2 * velocityX_OurRobot * velocityX_OpponentRobot + velocityX_OpponentRobot * velocityX_OpponentRobot + velocityY_diff * velocityY_diff); - if (minTime < 0) { - minTime = 0; - } - if (minTime > timeStep) { - minTime = timeStep; - } + minTime = std::clamp(minTime, 0.0, timeStep); - // check distance at minTime - Vector2 ourLocationAtMinTime = Vector2(startPointX_OurRobot, startPointY_OurRobot) + Vector2(velocityX_OurRobot, velocityY_OurRobot) * minTime; - Vector2 theirLocationAtMinTime = Vector2(startPointX_OpponentRobot, startPointY_OpponentRobot) + Vector2(velocityX_OpponentRobot, velocityY_OpponentRobot) * minTime; - double distance = (ourLocationAtMinTime - theirLocationAtMinTime).length(); + const Vector2 ourLocationAtMinTime = Vector2(startPointX_OurRobot, startPointY_OurRobot) + Vector2(velocityX_OurRobot, velocityY_OurRobot) * minTime; + const Vector2 theirLocationAtMinTime = + Vector2(startPointX_OpponentRobot, startPointY_OpponentRobot) + Vector2(velocityX_OpponentRobot, velocityY_OpponentRobot) * minTime; + const double distance = (ourLocationAtMinTime - theirLocationAtMinTime).length(); if (distance < avoidanceDistance) { insertCollisionData(collisionDatas, CollisionData{ourLocationAtMinTime, loopTime, "TheirRobotCollision"}); return; @@ -221,50 +184,64 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, } void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, - const std::unordered_map> &computedPaths, int robotId, double timeStep, size_t completedTimeSteps) { - auto ourRobots = world->getWorld()->getUs(); - const double maxCollisionCheckTime = 0.5; // Maximum time to check for collisions - const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; // Distance to avoid our robots - - for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { - if (completedTimeSteps + 1 >= pathPoints.size()) { + const std::unordered_map> &computedPaths, int robotId, double timeStep, + std::unordered_map completedTimeSteps) { + const auto ourRobots = world->getWorld()->getUs(); + const double maxCollisionCheckTime = 0.7; + const double avoidanceDistance = 2 * ai::Constants::ROBOT_RADIUS() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; + + for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps[robotId]++) { + if (completedTimeSteps[robotId] + 1 >= pathPoints.size()) { return; } + const double startPointX_OurRobot = pathPoints[completedTimeSteps[robotId]].x; + const double startPointY_OurRobot = pathPoints[completedTimeSteps[robotId]].y; + const double velocityX_OurRobot = (pathPoints[completedTimeSteps[robotId] + 1].x - startPointX_OurRobot) / timeStep; + const double velocityY_OurRobot = (pathPoints[completedTimeSteps[robotId] + 1].y - startPointY_OurRobot) / timeStep; + for (const auto &ourRobot : ourRobots) { - if (ourRobot->getId() == robotId) continue; - if ((pathPoints[completedTimeSteps] - (ourRobot->getPos() + ourRobot->getVel() * loopTime)).length() > 1) { + const int otherRobotId = ourRobot->getId(); + + if (otherRobotId == robotId) continue; + if ((pathPoints[completedTimeSteps[robotId]] - (ourRobot->getPos() + ourRobot->getVel() * loopTime)).length() > 1) { continue; } - double startPointX_OurRobot = pathPoints[completedTimeSteps].x; - double startPointY_OurRobot = pathPoints[completedTimeSteps].y; - double startPointX_OtherRobot = ourRobot->getPos().x + ourRobot->getVel().x * loopTime; - double startPointY_OtherRobot = ourRobot->getPos().y + ourRobot->getVel().y * loopTime; - - double velocityX_OurRobot = (pathPoints[completedTimeSteps + 1].x - startPointX_OurRobot) / timeStep; - double velocityY_OurRobot = (pathPoints[completedTimeSteps + 1].y - startPointY_OurRobot) / timeStep; - double velocityX_OtherRobot = ourRobot->getVel().x; - double velocityY_OtherRobot = ourRobot->getVel().y; - - double velocityX_diff = velocityX_OurRobot - velocityX_OtherRobot; - double velocityY_diff = velocityY_OurRobot - velocityY_OtherRobot; + const auto completedTimeStepsIt = completedTimeSteps.find(otherRobotId); + const auto computedPathsIt = computedPaths.find(otherRobotId); + + double startPointX_OtherRobot, startPointY_OtherRobot, velocityX_OtherRobot, velocityY_OtherRobot; + + if (completedTimeStepsIt != completedTimeSteps.end() && computedPathsIt != computedPaths.end() && completedTimeStepsIt->second + 1 < computedPathsIt->second.size()) { + startPointX_OtherRobot = computedPathsIt->second[completedTimeStepsIt->second].x; + startPointY_OtherRobot = computedPathsIt->second[completedTimeStepsIt->second].y; + velocityX_OtherRobot = (computedPathsIt->second[completedTimeStepsIt->second + 1].x - startPointX_OtherRobot) / timeStep; + velocityY_OtherRobot = (computedPathsIt->second[completedTimeStepsIt->second + 1].y - startPointY_OtherRobot) / timeStep; + } else { + if (computedPathsIt == computedPaths.end() || computedPathsIt->second.empty()) { + startPointX_OtherRobot = ourRobot->getPos().x; + startPointY_OtherRobot = ourRobot->getPos().y; + } else { + startPointX_OtherRobot = computedPathsIt->second.back().x; + startPointY_OtherRobot = computedPathsIt->second.back().y; + } + velocityX_OtherRobot = 0; + velocityY_OtherRobot = 0; + } + const double velocityX_diff = velocityX_OurRobot - velocityX_OtherRobot; + const double velocityY_diff = velocityY_OurRobot - velocityY_OtherRobot; double minTime = (startPointX_OurRobot * velocityX_diff + velocityX_OurRobot * startPointX_OtherRobot - startPointX_OtherRobot * velocityX_OtherRobot - startPointY_OurRobot * velocityY_OurRobot + startPointY_OurRobot * velocityY_OtherRobot + velocityY_OurRobot * startPointY_OtherRobot - startPointY_OtherRobot * velocityY_OtherRobot) / (velocityX_OurRobot * velocityX_OurRobot - 2 * velocityX_OurRobot * velocityX_OtherRobot + velocityX_OtherRobot * velocityX_OtherRobot + velocityY_diff * velocityY_diff); - if (minTime < 0) { - minTime = 0; - } - if (minTime > timeStep) { - minTime = timeStep; - } + minTime = std::clamp(minTime, 0.0, timeStep); - Vector2 ourLocationAtMinTime = Vector2(startPointX_OurRobot, startPointY_OurRobot) + Vector2(velocityX_OurRobot, velocityY_OurRobot) * minTime; - Vector2 otherRobotLocationAtMinTime = Vector2(startPointX_OtherRobot, startPointY_OtherRobot) + Vector2(velocityX_OtherRobot, velocityY_OtherRobot) * minTime; - double distance = (ourLocationAtMinTime - otherRobotLocationAtMinTime).length(); - if (distance < avoidanceDistance) { + const Vector2 ourLocationAtMinTime = Vector2(startPointX_OurRobot, startPointY_OurRobot) + Vector2(velocityX_OurRobot, velocityY_OurRobot) * minTime; + const Vector2 otherRobotLocationAtMinTime = Vector2(startPointX_OtherRobot, startPointY_OtherRobot) + Vector2(velocityX_OtherRobot, velocityY_OtherRobot) * minTime; + const double distanceSquared = (ourLocationAtMinTime - otherRobotLocationAtMinTime).length(); + if (distanceSquared < avoidanceDistance) { insertCollisionData(collisionDatas, CollisionData{ourLocationAtMinTime, loopTime, "OurRobotCollision"}); return; } @@ -276,7 +253,6 @@ void WorldObjects::calculateBallPlacementCollision(const rtt::world::World *worl double timeStep, size_t completedTimeSteps) { auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); auto startPositionBall = world->getWorld()->getBall()->get()->position; - // line segment from startPosition to ballPlacementPos LineSegment ballPlacementLine(startPositionBall, ballPlacementPos); double time = completedTimeSteps * timeStep; diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index c09a65732..efb34df46 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -49,25 +49,20 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: if (shouldRecalculateTrajectory(world, field, robotId, targetPosition, currentPosition, avoidObjects)) { computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - - // Check path to original target for collisions - firstCollision = worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects); - + completedTimeSteps[robotId] = 0; + firstCollision = worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects, completedTimeSteps); if (firstCollision.has_value()) { - // RTT_DEBUG(firstCollision->collisionName); - // Create intermediate points, return a collision-free trajectory originating from the best option of these points auto newTrajectory = findNewTrajectory(world, field, robotId, currentPosition, currentVelocity, firstCollision, targetPosition, maxRobotVelocity, timeStep, avoidObjects); if (newTrajectory.has_value()) { computedTrajectories[robotId] = newTrajectory.value(); } else { commandCollision.collisionData = firstCollision; - // RTT_DEBUG("Could not find a collision-free path"); } } computedPaths[robotId] = computedTrajectories[robotId].getPathApproach(timeStep); - computedPathsVel[robotId] = computedTrajectories[robotId].getVelocityVector(timeStep); // creates a vector with all the velocities + computedPathsVel[robotId] = computedTrajectories[robotId].getVelocityVector(timeStep); computedPathsPosVel[robotId].clear(); computedPathsPosVel[robotId].reserve(computedPaths[robotId].size()); for (size_t i = 0; i < computedPaths[robotId].size(); i++) { @@ -97,10 +92,10 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: }, computedPaths[robotId]); - // Current method is very hacky // If you are closer to the target than the first point of the approximated path, remove it if (computedPaths[robotId].size() > 1 && (targetPosition - currentPosition).length() < (targetPosition - computedPaths[robotId].front()).length()) { computedPaths[robotId].erase(computedPaths[robotId].begin()); + completedTimeSteps[robotId]++; } commandCollision.robotCommand = {}; @@ -108,8 +103,7 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: Position trackingVelocity = pathTrackingAlgorithmBBT.trackPathForwardAngle(currentPosition, currentVelocity, computedPathsPosVel[robotId], robotId, pidType); Vector2 trackingVelocityVector = {trackingVelocity.x, trackingVelocity.y}; - // If there is a collision on the path (so no collision-free path could be found), lower the speed to 1 m/s. This increases the chances of finding a new path - // while also decreasing the speed at which collisions happen + // If there is a collision on the path (so no collision-free path could be found), lower the speed to 1 m/s if (commandCollision.collisionData.has_value()) { if (trackingVelocityVector.length() > 1) trackingVelocityVector = trackingVelocityVector.stretchToLength(1); } @@ -119,21 +113,52 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: return commandCollision; } + +double PositionControl::calculateScore(const rtt::world::World *world, const rtt::Field &field, int robotId, std::optional &firstCollision, + Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects) { + double totalTime = trajectoryAroundCollision.getTotalTime(); + double score = totalTime; + if (!firstCollision.has_value()) { + return score; + } + + score += 5; + double timeTillFirstCollision = firstCollision.value().collisionTime; + score += std::max(0.0, 3 - timeTillFirstCollision); + + if (avoidObjects.shouldAvoidDefenseArea) { + auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, trajectoryAroundCollision, computedPaths, robotId); + if (defenseAreaCollision.has_value()) { + score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime) * 10; + score += 5; + } + } + + auto currentStrategyName = rtt::ai::GameStateManager::getCurrentGameState().getStrategyName(); + if (currentStrategyName == "ball_placement_them") { + auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); + auto startPositionBall = world->getWorld()->getBall()->get()->position; + Line ballPlacementLine(startPositionBall, ballPlacementPos); + Vector2 p1 = firstCollision.value().collisionPosition; + Vector2 p2 = trajectoryAroundCollision.getPosition(totalTime); + if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { + double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); + double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); + score += std::min(d1, d2) * 10; + score += 10; + } + } + + return score; +} + std::optional PositionControl::findNewTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 ¤tPosition, Vector2 ¤tVelocity, std::optional &firstCollision, Vector2 &targetPosition, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { - auto intermediatePoints = createIntermediatePoints(field, robotId, firstCollision, targetPosition); - // draw intermediate points on the field, uncomment for debugging - // rtt::ai::gui::Out::draw( - // { - // .label = "intermediate_points" + std::to_string(robotId), - // .color = proto::Drawing::YELLOW, - // .method = proto::Drawing::DOTS, - // .category = proto::Drawing::PATH_PLANNING, - // .forRobotId = robotId, - // .size = 2, - // }, - // intermediatePoints); + auto intermediatePoints = createIntermediatePoints(field, firstCollision, targetPosition); + // order intermediatepoints such that points closer to the target are first + std::sort(intermediatePoints.begin(), intermediatePoints.end(), + [&targetPosition](const Vector2 &a, const Vector2 &b) { return (a - targetPosition).length() < (b - targetPosition).length(); }); double bestScore = 999; std::optional bestTrajectory = std::nullopt; @@ -141,73 +166,12 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: for (const auto &intermediatePoint : intermediatePoints) { Trajectory2D trajectoryToIntermediatePoint(currentPosition, currentVelocity, intermediatePoint, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - auto intermediatePathCollision = worldObjects.getFirstCollision(world, field, trajectoryToIntermediatePoint, computedPaths, robotId, avoidObjects); + auto intermediatePathCollision = worldObjects.getFirstCollision(world, field, trajectoryToIntermediatePoint, computedPaths, robotId, avoidObjects, completedTimeSteps); auto trajectoryAroundCollision = calculateTrajectoryAroundCollision(world, field, intermediatePathCollision, trajectoryToIntermediatePoint, targetPosition, robotId, maxRobotVelocity, timeStep, avoidObjects); if (trajectoryAroundCollision.has_value()) { - auto firstCollision = worldObjects.getFirstCollision(world, field, trajectoryAroundCollision.value(), computedPaths, robotId, avoidObjects); - double totalTime = trajectoryAroundCollision.value().getTotalTime(); - double score = totalTime; - if (firstCollision.has_value()) { - score += 5; - double timeTillFirstCollision = firstCollision.value().collisionTime; - score += std::max(0.0, 3 - timeTillFirstCollision); - if (avoidObjects.shouldAvoidDefenseArea) { - auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, trajectoryAroundCollision.value(), computedPaths, robotId); - if (defenseAreaCollision.has_value()) { - score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime); - } - } - if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { - auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); - auto startPositionBall = world->getWorld()->getBall()->get()->position; - Line ballPlacementLine(startPositionBall, ballPlacementPos); - Vector2 p1 = firstCollision.value().collisionPosition; - Vector2 p2 = targetPosition; - if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { - double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); - double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); - if (field.leftDefenseArea.contains(ballPlacementPos, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT) || - field.rightDefenseArea.contains(ballPlacementPos, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT)) { - d1 += 999; - } - if (field.leftDefenseArea.contains(startPositionBall, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT) || - field.rightDefenseArea.contains(startPositionBall, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT)) { - d2 += 999; - } - score += std::min(d1, d2) * 10; - score += 10; - } - } - } - // plot the trajectory, uncomment for debugging - // double max_score = 50; - // double min_score = 0; - // score = (score - min_score) / (max_score - min_score); - // // if score is below 0.1, color is green, if score is above 0.5, color is red, else color is yellow - // auto color = score < 0.1 ? proto::Drawing::GREEN : score > 0.5 ? proto::Drawing::RED : proto::Drawing::YELLOW; - - // std::vector plotableTrajectory = trajectoryAroundCollision.value().getPathApproach(timeStep); - // rtt::ai::gui::Out::draw( - // { - // .label = "intermediate_trajectory" + std::to_string(robotId) + "_" + std::to_string(intermediatePoint.x) + "_" + std::to_string(intermediatePoint.y), - // .color = color, - // .method = proto::Drawing::LINES_CONNECTED, - // .category = proto::Drawing::PATH_PLANNING, - // .forRobotId = robotId, - // .thickness = std::max(int(1/score - 9), 0), - // }, - // plotableTrajectory); - // rtt::ai::gui::Out::draw( - // { - // .label = "intermediate_trajectory_dots" + std::to_string(robotId) + "_" + std::to_string(intermediatePoint.x) + "_" + std::to_string(intermediatePoint.y), - // .color = color, - // .method = proto::Drawing::DOTS, - // .category = proto::Drawing::PATH_PLANNING, - // .forRobotId = robotId, - // .size = 2, - // }, - // plotableTrajectory); + auto firstCollision = worldObjects.getFirstCollision(world, field, trajectoryAroundCollision.value(), computedPaths, robotId, avoidObjects, completedTimeSteps); + double score = calculateScore(world, field, robotId, firstCollision, trajectoryAroundCollision.value(), avoidObjects); if (score < bestScore) { bestScore = score; bestTrajectory = trajectoryAroundCollision.value(); @@ -226,62 +190,52 @@ std::optional PositionControl::calculateTrajectoryAroundCollision( std::optional bestTrajectory = std::nullopt; std::optional bestIntermediateToTarget = std::nullopt; - if (!intermediatePathCollision.has_value()) { - timeStep *= 2; - int numberOfTimeSteps = floor(trajectoryToIntermediatePoint.getTotalTime() / timeStep); - for (int i = 0; i < numberOfTimeSteps; i++) { - Vector2 newStart = trajectoryToIntermediatePoint.getPosition(i * timeStep); - Vector2 newVelocity = trajectoryToIntermediatePoint.getVelocity(i * timeStep); - - Trajectory2D intermediateToTarget(newStart, newVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - - auto newStartCollisions = worldObjects.getFirstCollision(world, field, intermediateToTarget, computedPaths, robotId, avoidObjects); - - if (newStartCollisions.has_value()) { - double totalTime = i * timeStep + intermediateToTarget.getTotalTime(); - double timeTillFirstCollision = newStartCollisions.value().collisionTime; - double score = totalTime + 5 + std::max(0.0, 3 - timeTillFirstCollision); - if (avoidObjects.shouldAvoidDefenseArea) { - auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, intermediateToTarget, computedPaths, robotId); - if (defenseAreaCollision.has_value()) { - score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime); - } - } + timeStep *= 2; + int numberOfTimeSteps = floor(trajectoryToIntermediatePoint.getTotalTime() / timeStep); + for (int i = 0; i < numberOfTimeSteps; i++) { + Vector2 newStart = trajectoryToIntermediatePoint.getPosition(i * timeStep); + Vector2 newVelocity = trajectoryToIntermediatePoint.getVelocity(i * timeStep); - if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { - auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); - auto startPositionBall = world->getWorld()->getBall()->get()->position; - Line ballPlacementLine(startPositionBall, ballPlacementPos); - Vector2 p1 = newStartCollisions.value().collisionPosition; - Vector2 p2 = targetPosition; - if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { - double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); - double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); - if (field.leftDefenseArea.contains(ballPlacementPos, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT) || - field.rightDefenseArea.contains(ballPlacementPos, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT)) { - d1 += 999; - } - if (field.leftDefenseArea.contains(startPositionBall, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT) || - field.rightDefenseArea.contains(startPositionBall, stp::control_constants::MINIMUM_DISTANCE_BETWEEN_GOAL_AND_PLACEMENT)) { - d2 += 999; - } - d2 += 999; - score += std::min(d1, d2) * 10; - score += 10; - } + Trajectory2D intermediateToTarget(newStart, newVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); + + auto newStartCollisions = worldObjects.getFirstCollision(world, field, intermediateToTarget, computedPaths, robotId, avoidObjects, completedTimeSteps); + + if (newStartCollisions.has_value()) { + double totalTime = i * timeStep + intermediateToTarget.getTotalTime(); + double timeTillFirstCollision = newStartCollisions.value().collisionTime; + double score = totalTime + 5 + std::max(0.0, 3 - timeTillFirstCollision); + if (avoidObjects.shouldAvoidDefenseArea) { + auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, intermediateToTarget, computedPaths, robotId); + if (defenseAreaCollision.has_value()) { + score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime); } + } - if (score < bestScore) { - bestScore = score; - bestTrajectory = trajectoryToIntermediatePoint; - bestIntermediateToTarget = intermediateToTarget; - bestTime = i * timeStep; + if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { + auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); + auto startPositionBall = world->getWorld()->getBall()->get()->position; + Line ballPlacementLine(startPositionBall, ballPlacementPos); + Vector2 p1 = newStartCollisions.value().collisionPosition; + Vector2 p2 = targetPosition; + if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { + double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); + double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); + d2 += 999; + score += std::min(d1, d2) * 10; + score += 10; } - } else { - trajectoryToIntermediatePoint.addTrajectory(intermediateToTarget, i * timeStep); - trajectoryToIntermediatePoint.getTotalTime(); - return trajectoryToIntermediatePoint; // This is now the whole path } + + if (score < bestScore) { + bestScore = score; + bestTrajectory = trajectoryToIntermediatePoint; + bestIntermediateToTarget = intermediateToTarget; + bestTime = i * timeStep; + } + } else { + trajectoryToIntermediatePoint.addTrajectory(intermediateToTarget, i * timeStep); + trajectoryToIntermediatePoint.getTotalTime(); + return trajectoryToIntermediatePoint; // This is now the whole path } } // If there was a collision in all intermediate path, return the best free trajectory @@ -291,30 +245,23 @@ std::optional PositionControl::calculateTrajectoryAroundCollision( return bestTrajectory; } -std::vector PositionControl::createIntermediatePoints(const rtt::Field &field, int robotId, std::optional &firstCollision, Vector2 &targetPosition) { - double angleBetweenIntermediatePoints = M_PI_4 / 2; - - // Radius and point extension of intermediate points are based on the fieldHeight - auto fieldHeight = field.playArea.height(); - - // PointToDrawFrom is picked by drawing a line from the target position to the obstacle and extending that - // line further towards our currentPosition by extension meters. - float pointExtension = fieldHeight / 18; // How far the pointToDrawFrom has to be from the collisionPosition - Vector2 pointToDrawFrom = firstCollision->collisionPosition + (firstCollision->collisionPosition - targetPosition).normalize() * pointExtension; +std::vector PositionControl::createIntermediatePoints(const rtt::Field &field, std::optional &firstCollision, Vector2 &targetPosition) { + float angleBetweenIntermediatePoints = M_PI_4 / 2; + float fieldHeight = field.playArea.height(); + float pointExtension = fieldHeight / 18; + Vector2 collisionToTargetNormalized = (firstCollision->collisionPosition - targetPosition).normalize(); + Vector2 pointToDrawFrom = firstCollision->collisionPosition + collisionToTargetNormalized * pointExtension; std::vector intermediatePoints; + float intermediatePointRadius = fieldHeight / 4; + Vector2 pointToRotate = pointToDrawFrom + collisionToTargetNormalized * intermediatePointRadius; for (int i = -9; i < 9; i++) { if (i != 0) { - // Make half circle of intermediatePoints pointed towards collisionPosition, originating from pointToDrawFrom, by rotating pointToRotate with a radius - // intermediatePointRadius - float intermediatePointRadius = fieldHeight / 4; // Radius of the half circle - Vector2 pointToRotate = pointToDrawFrom + (targetPosition - firstCollision->collisionPosition).normalize() * intermediatePointRadius; Vector2 intermediatePoint = pointToRotate.rotateAroundPoint(i * angleBetweenIntermediatePoints, pointToDrawFrom); - if (!field.playArea.contains(intermediatePoint)) { - continue; + if (field.playArea.contains(intermediatePoint)) { + intermediatePoints.emplace_back(intermediatePoint); } - intermediatePoints.emplace_back(intermediatePoint); } } return intermediatePoints; @@ -325,7 +272,7 @@ bool PositionControl::shouldRecalculateTrajectory(const rtt::world::World *world if (!computedTrajectories.contains(robotId) || (computedPaths.contains(robotId) && !computedPaths[robotId].empty() && (targetPosition - computedPaths[robotId][computedPaths[robotId].size() - 1]).length() > stp::control_constants::GO_TO_POS_ERROR_MARGIN) || - worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects).has_value()) { + worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects, completedTimeSteps).has_value()) { return true; } diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index 2a6d4a4e7..80f5f84da 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -28,10 +28,6 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { info.getCurrentWorld(), info.getField().value(), info.getRobot().value()->getId(), info.getRobot().value()->getPos(), info.getRobot().value()->getVel(), targetPos, info.getMaxRobotVelocity(), info.getPidType().value(), avoidObj); - if (commandCollision.collisionData.has_value()) { - // return Status::Failure; - } - double targetVelocityLength; if (info.getPidType() == stp::PIDType::KEEPER && (info.getRobot()->get()->getPos() - targetPos).length() > 2.0 * control_constants::ROBOT_RADIUS) { targetVelocityLength = std::max(std::clamp(commandCollision.robotCommand.velocity.length(), 0.0, info.getMaxRobotVelocity()), 1.5); // TODO: Tune this value better From bc1617c13308401354cbf0ac3e3703e1b2a532cb Mon Sep 17 00:00:00 2001 From: Jorn Date: Sun, 21 Jan 2024 20:17:09 +0100 Subject: [PATCH 09/17] Update collision avoidance planning --- .../BBTrajectories/WorldObjects.h | 4 +- .../control/positionControl/PositionControl.h | 16 --- .../BBTrajectories/WorldObjects.cpp | 8 +- .../positionControl/PositionControl.cpp | 103 ++++-------------- .../src/stp/tactics/passive/BlockBall.cpp | 2 +- 5 files changed, 34 insertions(+), 99 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h index 1d826043e..c51854d44 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h @@ -32,8 +32,8 @@ struct CollisionData { * @memberof collisionData Data about the collision */ struct CommandCollision { - RobotCommand robotCommand; - std::optional collisionData; + RobotCommand robotCommand = {}; + std::optional collisionData = std::nullopt; }; class WorldObjects { diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index dcba2eb55..5369c4cbb 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -137,22 +137,6 @@ class PositionControl { std::optional &firstCollision, Vector2 &targetPosition, double maxRobotVelocity, double timeStep, stp::AvoidObjects AvoidObjects); - /** - * @brief Calculates a path to the targetPosition from a point on the path to an intermediate path and - * return it if there are no collisions - * @param world the world object - * @param field the field object, used onwards by the collision detector - * @param intermediatePathCollision if intermediatePathCollision has no value, return {} - * @param pathToIntermediatePoint used for getting new start points of the BBT to the targetPosition - * @param targetPosition the desired position that the robot has to reach - * @param robotId the ID of the robot for which the path is calculated - * @param timeStep time in seconds between new start points on the BBT to the intermediatePoint - * @return optional Trajectory if a new path was found - */ - std::optional calculateTrajectoryAroundCollision(const rtt::world::World *world, const rtt::Field &field, - std::optional &intermediatePathCollision, Trajectory2D trajectoryToIntermediatePoint, - Vector2 &targetPosition, int robotId, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects); - /** * @brief Creates intermediate points to make a path to. These points all have equal distance to the * collision point diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 76ba3a2bc..b545d4503 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -141,7 +141,7 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, double timeStep, size_t completedTimeSteps, int avoidId) { const std::vector &theirRobots = world->getWorld()->getThem(); const double maxCollisionCheckTime = 0.7; - const double avoidanceDistance = 2.5 * ai::Constants::ROBOT_RADIUS_MAX() + ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; + const double avoidanceDistance = 4 * ai::Constants::ROBOT_RADIUS_MAX(); for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { if (completedTimeSteps + 1 >= pathPoints.size()) { @@ -163,6 +163,9 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, const double velocityX_diff = velocityX_OurRobot - velocityX_OpponentRobot; const double velocityY_diff = velocityY_OurRobot - velocityY_OpponentRobot; + if (velocityX_diff * velocityX_diff + velocityY_diff * velocityY_diff < 1) { + continue; + } double minTime = (startPointX_OurRobot * velocityX_diff + velocityX_OurRobot * startPointX_OpponentRobot - startPointX_OpponentRobot * velocityX_OpponentRobot - startPointY_OurRobot * velocityY_OurRobot + startPointY_OurRobot * velocityY_OpponentRobot + velocityY_OurRobot * startPointY_OpponentRobot - @@ -230,6 +233,9 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s } const double velocityX_diff = velocityX_OurRobot - velocityX_OtherRobot; const double velocityY_diff = velocityY_OurRobot - velocityY_OtherRobot; + if (velocityX_diff * velocityX_diff + velocityY_diff * velocityY_diff < 1) { + continue; + } double minTime = (startPointX_OurRobot * velocityX_diff + velocityX_OurRobot * startPointX_OtherRobot - startPointX_OtherRobot * velocityX_OtherRobot - startPointY_OurRobot * velocityY_OurRobot + startPointY_OurRobot * velocityY_OtherRobot + velocityY_OurRobot * startPointY_OtherRobot - diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index efb34df46..553f6f55d 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -45,7 +45,7 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: double timeStep = 0.1; std::optional firstCollision; - rtt::BB::CommandCollision commandCollision; + rtt::BB::CommandCollision commandCollision = {}; if (shouldRecalculateTrajectory(world, field, robotId, targetPosition, currentPosition, avoidObjects)) { computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); @@ -56,7 +56,9 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: findNewTrajectory(world, field, robotId, currentPosition, currentVelocity, firstCollision, targetPosition, maxRobotVelocity, timeStep, avoidObjects); if (newTrajectory.has_value()) { computedTrajectories[robotId] = newTrajectory.value(); - } else { + } + firstCollision = worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects, completedTimeSteps); + if (firstCollision.has_value()) { commandCollision.collisionData = firstCollision; } } @@ -98,14 +100,14 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: completedTimeSteps[robotId]++; } - commandCollision.robotCommand = {}; // Position trackingVelocity = pathTrackingAlgorithm.trackPathDefaultAngle(currentPosition, currentVelocity,computedPaths[robotId], robotId, pidType); Position trackingVelocity = pathTrackingAlgorithmBBT.trackPathForwardAngle(currentPosition, currentVelocity, computedPathsPosVel[robotId], robotId, pidType); Vector2 trackingVelocityVector = {trackingVelocity.x, trackingVelocity.y}; - // If there is a collision on the path (so no collision-free path could be found), lower the speed to 1 m/s - if (commandCollision.collisionData.has_value()) { - if (trackingVelocityVector.length() > 1) trackingVelocityVector = trackingVelocityVector.stretchToLength(1); + // Break if all paths result in collision and we will collide with a robot + if (commandCollision.collisionData.has_value() && + (commandCollision.collisionData.value().collisionName == "TheirRobotCollision" || commandCollision.collisionData.value().collisionName == "OurRobotCollision")) { + if (trackingVelocityVector.length() > 0.1) trackingVelocityVector = trackingVelocityVector.stretchToLength(0.1); } commandCollision.robotCommand.velocity = trackingVelocityVector; @@ -156,9 +158,9 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: Vector2 ¤tVelocity, std::optional &firstCollision, Vector2 &targetPosition, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { auto intermediatePoints = createIntermediatePoints(field, firstCollision, targetPosition); - // order intermediatepoints such that points closer to the target are first std::sort(intermediatePoints.begin(), intermediatePoints.end(), [&targetPosition](const Vector2 &a, const Vector2 &b) { return (a - targetPosition).length() < (b - targetPosition).length(); }); + timeStep *= 3; double bestScore = 999; std::optional bestTrajectory = std::nullopt; @@ -167,81 +169,24 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: Trajectory2D trajectoryToIntermediatePoint(currentPosition, currentVelocity, intermediatePoint, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); auto intermediatePathCollision = worldObjects.getFirstCollision(world, field, trajectoryToIntermediatePoint, computedPaths, robotId, avoidObjects, completedTimeSteps); - auto trajectoryAroundCollision = calculateTrajectoryAroundCollision(world, field, intermediatePathCollision, trajectoryToIntermediatePoint, targetPosition, robotId, - maxRobotVelocity, timeStep, avoidObjects); - if (trajectoryAroundCollision.has_value()) { - auto firstCollision = worldObjects.getFirstCollision(world, field, trajectoryAroundCollision.value(), computedPaths, robotId, avoidObjects, completedTimeSteps); - double score = calculateScore(world, field, robotId, firstCollision, trajectoryAroundCollision.value(), avoidObjects); - if (score < bestScore) { - bestScore = score; - bestTrajectory = trajectoryAroundCollision.value(); - } - } - } - return bestTrajectory; -} - -std::optional PositionControl::calculateTrajectoryAroundCollision(const rtt::world::World *world, const rtt::Field &field, - std::optional &intermediatePathCollision, - Trajectory2D trajectoryToIntermediatePoint, Vector2 &targetPosition, int robotId, - double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { - double bestScore = 999; - double bestTime = 0; - std::optional bestTrajectory = std::nullopt; - std::optional bestIntermediateToTarget = std::nullopt; - - timeStep *= 2; - int numberOfTimeSteps = floor(trajectoryToIntermediatePoint.getTotalTime() / timeStep); - for (int i = 0; i < numberOfTimeSteps; i++) { - Vector2 newStart = trajectoryToIntermediatePoint.getPosition(i * timeStep); - Vector2 newVelocity = trajectoryToIntermediatePoint.getVelocity(i * timeStep); - - Trajectory2D intermediateToTarget(newStart, newVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - - auto newStartCollisions = worldObjects.getFirstCollision(world, field, intermediateToTarget, computedPaths, robotId, avoidObjects, completedTimeSteps); - - if (newStartCollisions.has_value()) { - double totalTime = i * timeStep + intermediateToTarget.getTotalTime(); - double timeTillFirstCollision = newStartCollisions.value().collisionTime; - double score = totalTime + 5 + std::max(0.0, 3 - timeTillFirstCollision); - if (avoidObjects.shouldAvoidDefenseArea) { - auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, intermediateToTarget, computedPaths, robotId); - if (defenseAreaCollision.has_value()) { - score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime); - } - } - - if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { - auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); - auto startPositionBall = world->getWorld()->getBall()->get()->position; - Line ballPlacementLine(startPositionBall, ballPlacementPos); - Vector2 p1 = newStartCollisions.value().collisionPosition; - Vector2 p2 = targetPosition; - if (ballPlacementLine.arePointsOnOppositeSides(p1, p2)) { - double d1 = (p1 - ballPlacementPos).length() + (p2 - ballPlacementPos).length(); - double d2 = (p1 - startPositionBall).length() + (p2 - startPositionBall).length(); - d2 += 999; - score += std::min(d1, d2) * 10; - score += 10; + double maxLoopTime = intermediatePathCollision.has_value() ? intermediatePathCollision.value().collisionTime : trajectoryToIntermediatePoint.getTotalTime(); + for (double loopTime = 0; loopTime < maxLoopTime; loopTime += timeStep) { + Vector2 newStartPosition = trajectoryToIntermediatePoint.getPosition(loopTime); + Vector2 newStartVelocity = trajectoryToIntermediatePoint.getVelocity(loopTime); + Trajectory2D trajectoryAroundCollision(newStartPosition, newStartVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); + auto firstCollision = worldObjects.getFirstCollision(world, field, trajectoryAroundCollision, computedPaths, robotId, avoidObjects, completedTimeSteps); + if (!firstCollision.has_value()) { + trajectoryToIntermediatePoint.addTrajectory(trajectoryAroundCollision, loopTime); + return trajectoryToIntermediatePoint; + } else { + double score = calculateScore(world, field, robotId, firstCollision, trajectoryAroundCollision, avoidObjects); + if (score < bestScore) { + bestScore = score; + bestTrajectory = trajectoryToIntermediatePoint; } } - - if (score < bestScore) { - bestScore = score; - bestTrajectory = trajectoryToIntermediatePoint; - bestIntermediateToTarget = intermediateToTarget; - bestTime = i * timeStep; - } - } else { - trajectoryToIntermediatePoint.addTrajectory(intermediateToTarget, i * timeStep); - trajectoryToIntermediatePoint.getTotalTime(); - return trajectoryToIntermediatePoint; // This is now the whole path } } - // If there was a collision in all intermediate path, return the best free trajectory - if (bestTrajectory.has_value()) { - bestTrajectory.value().addTrajectory(bestIntermediateToTarget.value(), bestTime); - } return bestTrajectory; } @@ -255,7 +200,7 @@ std::vector PositionControl::createIntermediatePoints(const rtt::Field std::vector intermediatePoints; float intermediatePointRadius = fieldHeight / 4; Vector2 pointToRotate = pointToDrawFrom + collisionToTargetNormalized * intermediatePointRadius; - for (int i = -9; i < 9; i++) { + for (int i = -6; i < 7; i++) { if (i != 0) { Vector2 intermediatePoint = pointToRotate.rotateAroundPoint(i * angleBetweenIntermediatePoints, pointToDrawFrom); diff --git a/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp b/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp index 0c13dbb57..df1f746d8 100644 --- a/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp @@ -72,7 +72,7 @@ Vector2 BlockBall::calculateTargetPosition(const world::view::BallView &ball, Ve } // Do not get closer than 4 robot radii (to avoid collisions) - distance = std::max(4 * control_constants::ROBOT_RADIUS, distance); + distance = std::max(4 * control_constants::ROBOT_RADIUS + control_constants::GO_TO_POS_ERROR_MARGIN, distance); return defendPos + targetToBall.stretchToLength(distance); } From b32621ac63f87d6864659e6ec86b79c9129c14ca Mon Sep 17 00:00:00 2001 From: Jorn Date: Sun, 21 Jan 2024 20:34:51 +0100 Subject: [PATCH 10/17] Update collision name (str) to collision type (enum) --- .../positionControl/BBTrajectories/WorldObjects.h | 9 +++++++-- .../positionControl/BBTrajectories/WorldObjects.cpp | 12 ++++++------ .../src/control/positionControl/PositionControl.cpp | 4 ++-- 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h index c51854d44..e14aef2cd 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h @@ -14,16 +14,21 @@ namespace rtt::BB { +/** + * @brief Enum that stores the different types of collisions + */ +enum class CollisionType { FIELD, DEFENSEAREA, BALL, ENEMYROBOT, OWNROBOT, BALLPLACEMENT }; + /** * @brief Struct that stores data about a collision * @memberof collisionPosition position robot shouldn't come * @memberof collisionTime number of seconds from now that the collision will occur - * @memberof collisionName the name of what causes the collision + * @memberof collisionType type of collision */ struct CollisionData { Vector2 collisionPosition; double collisionTime; - std::string collisionName; + CollisionType collisionType; }; /** diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index b545d4503..35dd083fb 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -69,7 +69,7 @@ void WorldObjects::calculateFieldCollisions(const rtt::Field &field, std::vector // Don't care about the field if the robot is already outside the field (i == 0 is the first point of the robot's path, so almost the currentPosition). if (i == 0) return; - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), "FieldCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), BB::CollisionType::FIELD}); return; } } @@ -86,7 +86,7 @@ void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std:: // Don't care about the defense area if the robot is already in the defense area. It should just get out as fast as possible :) // if (i == 0) return; - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), "DefenseAreaCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), BB::CollisionType::DEFENSEAREA}); return; } } @@ -131,7 +131,7 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: double distance = (ourLocationAtMinTime - theirLocationAtMinTime).length(); if (distance < dist) { - insertCollisionData(collisionDatas, CollisionData{pathPoints[completedTimeSteps], loopTime, "BallCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[completedTimeSteps], loopTime, BB::CollisionType::BALL}); return; } } @@ -179,7 +179,7 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, Vector2(startPointX_OpponentRobot, startPointY_OpponentRobot) + Vector2(velocityX_OpponentRobot, velocityY_OpponentRobot) * minTime; const double distance = (ourLocationAtMinTime - theirLocationAtMinTime).length(); if (distance < avoidanceDistance) { - insertCollisionData(collisionDatas, CollisionData{ourLocationAtMinTime, loopTime, "TheirRobotCollision"}); + insertCollisionData(collisionDatas, CollisionData{ourLocationAtMinTime, loopTime, BB::CollisionType::ENEMYROBOT}); return; } } @@ -248,7 +248,7 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s const Vector2 otherRobotLocationAtMinTime = Vector2(startPointX_OtherRobot, startPointY_OtherRobot) + Vector2(velocityX_OtherRobot, velocityY_OtherRobot) * minTime; const double distanceSquared = (ourLocationAtMinTime - otherRobotLocationAtMinTime).length(); if (distanceSquared < avoidanceDistance) { - insertCollisionData(collisionDatas, CollisionData{ourLocationAtMinTime, loopTime, "OurRobotCollision"}); + insertCollisionData(collisionDatas, CollisionData{ourLocationAtMinTime, loopTime, BB::CollisionType::OWNROBOT}); return; } } @@ -264,7 +264,7 @@ void WorldObjects::calculateBallPlacementCollision(const rtt::world::World *worl for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { if (ballPlacementLine.distanceToLine(pathPoints[i]) < ai::stp::control_constants::AVOID_BALL_DISTANCE) { - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), "BallPlacementCollision"}); + insertCollisionData(collisionDatas, CollisionData{pathPoints[i], timeStep * (i - completedTimeSteps), BB::CollisionType::BALLPLACEMENT}); return; } } diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 553f6f55d..36e66f43e 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -105,8 +105,8 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: Vector2 trackingVelocityVector = {trackingVelocity.x, trackingVelocity.y}; // Break if all paths result in collision and we will collide with a robot - if (commandCollision.collisionData.has_value() && - (commandCollision.collisionData.value().collisionName == "TheirRobotCollision" || commandCollision.collisionData.value().collisionName == "OurRobotCollision")) { + if (commandCollision.collisionData.has_value() && (commandCollision.collisionData.value().collisionType == BB::CollisionType::ENEMYROBOT || + commandCollision.collisionData.value().collisionType == BB::CollisionType::OWNROBOT)) { if (trackingVelocityVector.length() > 0.1) trackingVelocityVector = trackingVelocityVector.stretchToLength(0.1); } From fd1ffc613e25dc358828e31103d89abf4743a940 Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 22 Jan 2024 16:48:29 +0100 Subject: [PATCH 11/17] Include startTime to collision detection to make sure time axis allign and scoring is consistent --- .../BBTrajectories/WorldObjects.h | 14 ++++++--- .../control/positionControl/PositionControl.h | 4 ++- .../BBTrajectories/WorldObjects.cpp | 31 ++++++++++--------- .../positionControl/PositionControl.cpp | 13 ++++---- 4 files changed, 35 insertions(+), 27 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h index e14aef2cd..9cb845a67 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h @@ -62,12 +62,13 @@ class WorldObjects { * @param computedPaths the paths of our robots * @param robotId Id of the robot * @param avoidObjects a struct with if it should avoid certain objects - * @param completedTimeSteps Number of completed time steps + * @param completedTimeSteps Number of completed time steps of the trajectory + * @param startTime Start time of the current trajectory part that is being checked. This is the time that it takes for the robot to reach the first point of the trajectory * @return optional with rtt::BB::CollisionData */ std::optional getFirstCollision(const rtt::world::World *world, const rtt::Field &field, const rtt::Trajectory2D &Trajectory, const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects, - std::unordered_map completedTimeSteps); + std::unordered_map completedTimeSteps, double startTime = 0); /** * @brief Calculates the position of the first collision with the defense area, note that this functions assumes the entire trajectory, @@ -116,9 +117,10 @@ class WorldObjects { * @param timeStep Time between pathpoints * @param dist Distance to the ball * @param completedTimeSteps Number of completed time steps + * @param startTime Start time of the current trajectory part that is being checked. This is the time that it takes for the robot to reach the first point of the trajectory */ void calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, double dist, - size_t completedTimeSteps); + size_t completedTimeSteps, double startTime); /** * @brief Takes a calculated path of a robot and checks points along the path if they are too close to an @@ -130,9 +132,10 @@ class WorldObjects { * @param timeStep Time between pathpoints * @param completedTimeSteps Amount of seconds of the trajectory already completed by the robot * @param avoidId ID of the robot to ignore collisions with + * @param startTime Start time of the current trajectory part that is being checked. This is the time that it takes for the robot to reach the first point of the trajectory */ void calculateEnemyRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, - size_t completedTimeSteps, int avoidId); + size_t completedTimeSteps, int avoidId, double startTime); /** * @brief Takes a path from the array of stored paths and checks points along the path if they are too close to @@ -145,10 +148,11 @@ class WorldObjects { * @param robotId ID of the robot * @param timeStep Time between pathpoints * @param completedTimeSteps how many seconds of the trajectory is already completed by the robot + * @param startTime Start time of the current trajectory part that is being checked. This is the time that it takes for the robot to reach the first point of the trajectory */ void calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, const std::unordered_map> &computedPaths, int robotId, double timeStep, - std::unordered_map completedTimeSteps); + std::unordered_map completedTimeSteps, double startTime); /** * @brief Takes a calculated path of a robot and checks points along the path if they are too close to an diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index 5369c4cbb..6c506e457 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -113,10 +113,12 @@ class PositionControl { * @param robotId the ID of the robot for which the path is calculated * @param firstCollision location of the first collision on the current path * @param trajectoryAroundCollision the trajectory to the intermediate point + * @param avoidObjects whether or not to avoid objects + * @param startTime the time at which the trajectory starts * @return A score for the trajectory */ double calculateScore(const rtt::world::World *world, const rtt::Field &field, int robotId, std::optional &firstCollision, - Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects); + Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects, double startTime = 0); /** * @brief Tries to find a new trajectory when the current path has a collision on it. It tries this by diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 35dd083fb..b7bb67579 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -15,7 +15,7 @@ WorldObjects::WorldObjects() = default; std::optional WorldObjects::getFirstCollision(const rtt::world::World *world, const rtt::Field &field, const Trajectory2D &Trajectory, const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects, - std::unordered_map completedTimeSteps) { + std::unordered_map completedTimeSteps, double startTime) { double timeStep = 0.1; auto pathPoints = Trajectory.getPathApproach(timeStep); @@ -28,13 +28,13 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep, completedTimeSteps[robotId]); } if (avoidObjects.shouldAvoidBall) { - calculateBallCollisions(world, collisionDatas, pathPoints, timeStep, avoidObjects.avoidBallDist, completedTimeSteps[robotId]); + calculateBallCollisions(world, collisionDatas, pathPoints, timeStep, avoidObjects.avoidBallDist, completedTimeSteps[robotId], startTime); } if (avoidObjects.shouldAvoidTheirRobots || avoidObjects.notAvoidTheirRobotId != -1) { - calculateEnemyRobotCollisions(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId], avoidObjects.notAvoidTheirRobotId); + calculateEnemyRobotCollisions(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId], avoidObjects.notAvoidTheirRobotId, startTime); } if (avoidObjects.shouldAvoidOurRobots) { - calculateOurRobotCollisions(world, collisionDatas, pathPoints, computedPaths, robotId, timeStep, completedTimeSteps); + calculateOurRobotCollisions(world, collisionDatas, pathPoints, computedPaths, robotId, timeStep, completedTimeSteps, startTime); } if (rtt::ai::GameStateManager::getCurrentGameState().getStrategyName() == "ball_placement_them") { calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); @@ -93,11 +93,11 @@ void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std:: } void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, - double dist, size_t completedTimeSteps) { - double ballAvoidanceTime = 0.7; + double dist, size_t completedTimeSteps, double startTime) { + double ballAvoidanceTime = 0.7 - startTime; - const Vector2 &startPositionBall = world->getWorld()->getBall()->get()->position; const Vector2 &VelocityBall = world->getWorld()->getBall()->get()->velocity; + const Vector2 &startPositionBall = world->getWorld()->getBall()->get()->position + VelocityBall * startTime; for (double loopTime = 0; loopTime < ballAvoidanceTime; loopTime += timeStep, completedTimeSteps++) { if (completedTimeSteps + 1 >= pathPoints.size()) { @@ -138,9 +138,9 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: } void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, - double timeStep, size_t completedTimeSteps, int avoidId) { + double timeStep, size_t completedTimeSteps, int avoidId, double startTime) { const std::vector &theirRobots = world->getWorld()->getThem(); - const double maxCollisionCheckTime = 0.7; + const double maxCollisionCheckTime = 0.7 - startTime; const double avoidanceDistance = 4 * ai::Constants::ROBOT_RADIUS_MAX(); for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { @@ -156,10 +156,10 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, const double velocityY_OurRobot = (nextPoint.y - startPointY_OurRobot) / timeStep; for (const auto &opponentRobot : theirRobots) { - const double startPointX_OpponentRobot = opponentRobot->getPos().x + opponentRobot->getVel().x * loopTime; - const double startPointY_OpponentRobot = opponentRobot->getPos().y + opponentRobot->getVel().y * loopTime; const double velocityX_OpponentRobot = opponentRobot->getVel().x; const double velocityY_OpponentRobot = opponentRobot->getVel().y; + const double startPointX_OpponentRobot = opponentRobot->getPos().x + velocityX_OpponentRobot * (loopTime + startTime); + const double startPointY_OpponentRobot = opponentRobot->getPos().y + velocityY_OpponentRobot * (loopTime + startTime); const double velocityX_diff = velocityX_OurRobot - velocityX_OpponentRobot; const double velocityY_diff = velocityY_OurRobot - velocityY_OpponentRobot; @@ -188,9 +188,9 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, const std::unordered_map> &computedPaths, int robotId, double timeStep, - std::unordered_map completedTimeSteps) { + std::unordered_map completedTimeSteps, double startTime) { const auto ourRobots = world->getWorld()->getUs(); - const double maxCollisionCheckTime = 0.7; + const double maxCollisionCheckTime = 0.7 - startTime; const double avoidanceDistance = 2 * ai::Constants::ROBOT_RADIUS() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps[robotId]++) { @@ -206,11 +206,14 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s const int otherRobotId = ourRobot->getId(); if (otherRobotId == robotId) continue; - if ((pathPoints[completedTimeSteps[robotId]] - (ourRobot->getPos() + ourRobot->getVel() * loopTime)).length() > 1) { + if ((pathPoints[completedTimeSteps[robotId]] - (ourRobot->getPos() + ourRobot->getVel() * (loopTime + startTime))).length() > 1) { continue; } const auto completedTimeStepsIt = completedTimeSteps.find(otherRobotId); + if (completedTimeStepsIt != completedTimeSteps.end()) { + completedTimeStepsIt->second += static_cast(std::round(startTime / timeStep)); + } const auto computedPathsIt = computedPaths.find(otherRobotId); double startPointX_OtherRobot, startPointY_OtherRobot, velocityX_OtherRobot, velocityY_OtherRobot; diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 36e66f43e..f8c8a6fbf 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -117,21 +117,20 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: } double PositionControl::calculateScore(const rtt::world::World *world, const rtt::Field &field, int robotId, std::optional &firstCollision, - Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects) { + Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects, double startTime) { double totalTime = trajectoryAroundCollision.getTotalTime(); - double score = totalTime; + double score = totalTime + startTime; if (!firstCollision.has_value()) { return score; } score += 5; - double timeTillFirstCollision = firstCollision.value().collisionTime; - score += std::max(0.0, 3 - timeTillFirstCollision); + score += std::max(0.0, 3 - firstCollision.value().collisionTime - startTime); if (avoidObjects.shouldAvoidDefenseArea) { auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, trajectoryAroundCollision, computedPaths, robotId); if (defenseAreaCollision.has_value()) { - score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime) * 10; + score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime - startTime) * 10; score += 5; } } @@ -174,12 +173,12 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: Vector2 newStartPosition = trajectoryToIntermediatePoint.getPosition(loopTime); Vector2 newStartVelocity = trajectoryToIntermediatePoint.getVelocity(loopTime); Trajectory2D trajectoryAroundCollision(newStartPosition, newStartVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - auto firstCollision = worldObjects.getFirstCollision(world, field, trajectoryAroundCollision, computedPaths, robotId, avoidObjects, completedTimeSteps); + auto firstCollision = worldObjects.getFirstCollision(world, field, trajectoryAroundCollision, computedPaths, robotId, avoidObjects, completedTimeSteps, loopTime); if (!firstCollision.has_value()) { trajectoryToIntermediatePoint.addTrajectory(trajectoryAroundCollision, loopTime); return trajectoryToIntermediatePoint; } else { - double score = calculateScore(world, field, robotId, firstCollision, trajectoryAroundCollision, avoidObjects); + double score = calculateScore(world, field, robotId, firstCollision, trajectoryAroundCollision, avoidObjects, loopTime); if (score < bestScore) { bestScore = score; bestTrajectory = trajectoryToIntermediatePoint; From 9f3a88386ee6ec21c456917b4c5a35761230ddab Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 22 Jan 2024 16:50:46 +0100 Subject: [PATCH 12/17] - Increase distance to add a small error margin for distance to ball during ball avoidance - Fix ids of robots to avoid for defenders - Remove distance to edges of ball placement in score when ball is near target --- .../include/roboteam_ai/stp/constants/ControlConstants.h | 4 +++- roboteam_ai/src/control/positionControl/PositionControl.cpp | 3 +++ roboteam_ai/src/stp/computations/PositionComputations.cpp | 4 +--- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index f8efbd460..71dda0fa9 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -69,12 +69,14 @@ constexpr double ROBOT_CLOSE_TO_POINT = 0.2; /**< Distance from constexpr double DISTANCE_TO_ROBOT_NEAR = 2.2 * ROBOT_RADIUS; /**< Distance from the robot to another robot at which the robot is considered near that other robot */ constexpr double DEFENSE_AREA_AVOIDANCE_MARGIN = 0.1; /**< Distance error for avoiding the defense area */ constexpr double DISTANCE_TO_PASS_TRAJECTORY = 0.5; /**< Distance from the robot to the pass trajectory at which the robot is considered too close to the pass trajectory */ +constexpr double BALL_PLACEMENT_ALMOST_DONE_DISTANCE = + 0.5; /**< Distance between ball and placement location at which we no longer include the distance to the edges of the placement location in scoring the trajectory */ /// Keeper constants constexpr double DISTANCE_FROM_GOAL_CLOSE = 2 * ROBOT_RADIUS; /**< Distance from the keeper to the goal at which the keeper is considered close to that goal */ /// GameState constants -constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS; /**< Minimum distance all robots should keep when avoiding the ball */ +constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.1; /**< Minimum distance all robots should keep when avoiding the ball */ } // namespace rtt::ai::stp::control_constants diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index f8c8a6fbf..be3a3109c 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -139,6 +139,9 @@ double PositionControl::calculateScore(const rtt::world::World *world, const rtt if (currentStrategyName == "ball_placement_them") { auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); auto startPositionBall = world->getWorld()->getBall()->get()->position; + if ((startPositionBall - ballPlacementPos).length() < stp::control_constants::BALL_PLACEMENT_ALMOST_DONE_DISTANCE) { + return score; + } Line ballPlacementLine(startPositionBall, ballPlacementPos); Vector2 p1 = firstCollision.value().collisionPosition; Vector2 p2 = trajectoryAroundCollision.getPosition(totalTime); diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 374151054..974fa56d2 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -285,7 +285,6 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma erase_if(enemyRobots, [&](const auto enemyRobot) -> bool { return enemyClosestToBall && enemyRobot->getId() == enemyClosestToBall.value()->getId(); }); std::map enemyMap; std::vector enemies; - std::vector ids; for (auto enemy : enemyRobots) { if (enemy->hasBall()) continue; double score = FieldComputations::getDistanceToGoal(field, true, enemy->getPos()); @@ -295,7 +294,6 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma } EnemyInfo info = {enemy->getPos(), enemy->getVel(), enemy->getId()}; enemyMap.insert({score, info}); - ids.emplace_back(enemy->getId()); } ComputationManager::calculatedEnemyMapIds.clear(); // If defenders do not have a position yet, don't do hungarian algorithm @@ -354,7 +352,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma (stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos() - enemies[assignments[i]]).length() < IGNORE_COLLISIONS_DISTANCE && stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos().dist(field.leftGoalArea.rightLine().center()) < enemies[assignments[i]].dist(field.leftGoalArea.rightLine().center())) { - stpInfos[activeDefenderNames[i]].setNotAvoidTheirRobotId(ids[assignments[i]]); + stpInfos[activeDefenderNames[i]].setNotAvoidTheirRobotId(ComputationManager::calculatedEnemyMapIds[assignments[i]]); } else { stpInfos[activeDefenderNames[i]].setNotAvoidTheirRobotId(-1); } From 1c510d2d55e688ccb01fc122daa3afb803a57fb8 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 23 Jan 2024 17:35:54 +0100 Subject: [PATCH 13/17] Actually use not avoid id --- .../BBTrajectories/WorldObjects.cpp | 3 +++ .../src/stp/computations/PositionComputations.cpp | 14 +------------- 2 files changed, 4 insertions(+), 13 deletions(-) diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index b7bb67579..7d1b37065 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -156,6 +156,9 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, const double velocityY_OurRobot = (nextPoint.y - startPointY_OurRobot) / timeStep; for (const auto &opponentRobot : theirRobots) { + if (avoidId != -1 && opponentRobot->getId() == avoidId) { + continue; + } const double velocityX_OpponentRobot = opponentRobot->getVel().x; const double velocityY_OpponentRobot = opponentRobot->getVel().y; const double startPointX_OpponentRobot = opponentRobot->getPos().x + velocityX_OpponentRobot * (loopTime + startTime); diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 974fa56d2..d26b74b2c 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -242,12 +242,9 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_mapget()->hasBall() && enemyAngle.shortestAngleDiff(enemyToGoalAngle) > M_PI / 2) { auto enemyPos = enemyClosestToBall->get()->getPos(); - auto targetPos = FieldComputations::projectPointToValidPositionOnLine( - field, enemyPos - (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(control_constants::ROBOT_RADIUS), enemyPos, - enemyPos - (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(10), AvoidObjects{}); + auto targetPos = enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(control_constants::ROBOT_RADIUS * 4); stpInfos["harasser"].setPositionToMoveTo(targetPos); stpInfos["harasser"].setAngle((ballPos - targetPos).angle()); - // Maybe reset such that we go to formation tactic? } else { if (enemyClosestToBall->get()->getPos().dist(field.leftGoalArea.rightLine().center()) > stpInfos["harasser"].getRobot()->get()->getPos().dist(field.leftGoalArea.rightLine().center())) { @@ -347,15 +344,6 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma } else { stpInfos[activeDefenderNames[i]].setPositionToDefend(enemies[assignments[i]]); stpInfos[activeDefenderNames[i]].setBlockDistance(BlockDistance::ROBOTRADIUS); - constexpr double IGNORE_COLLISIONS_DISTANCE = 0.4; - if (stpInfos[activeDefenderNames[i]].getRobot() && - (stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos() - enemies[assignments[i]]).length() < IGNORE_COLLISIONS_DISTANCE && - stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos().dist(field.leftGoalArea.rightLine().center()) < - enemies[assignments[i]].dist(field.leftGoalArea.rightLine().center())) { - stpInfos[activeDefenderNames[i]].setNotAvoidTheirRobotId(ComputationManager::calculatedEnemyMapIds[assignments[i]]); - } else { - stpInfos[activeDefenderNames[i]].setNotAvoidTheirRobotId(-1); - } } } } From d6d371346816f29137f0d804f7562d7f4b7f308d Mon Sep 17 00:00:00 2001 From: Jorn Date: Wed, 24 Jan 2024 10:09:23 +0100 Subject: [PATCH 14/17] Fix robot avoidance settings in KeeperKickBall play --- roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp index e3f0f7098..21d74b257 100644 --- a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp +++ b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp @@ -73,6 +73,8 @@ void KeeperKickBall::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo.passLocation); + stpInfos["keeper"].setShouldAvoidTheirRobots(false); + stpInfos["keeper"].setShouldAvoidOurRobots(false); if (!ballKicked()) { stpInfos["receiver"].setPositionToMoveTo(passInfo.passLocation); From f892aff8361dd4c4597960eb9a42a5f980ebaf7b Mon Sep 17 00:00:00 2001 From: Jorn Date: Wed, 24 Jan 2024 16:25:38 +0100 Subject: [PATCH 15/17] Increase out of field margin for path planning --- .../include/roboteam_ai/stp/constants/ControlConstants.h | 1 + .../control/positionControl/BBTrajectories/WorldObjects.cpp | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index 71dda0fa9..6aa864122 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -71,6 +71,7 @@ constexpr double DEFENSE_AREA_AVOIDANCE_MARGIN = 0.1; /**< Distance erro constexpr double DISTANCE_TO_PASS_TRAJECTORY = 0.5; /**< Distance from the robot to the pass trajectory at which the robot is considered too close to the pass trajectory */ constexpr double BALL_PLACEMENT_ALMOST_DONE_DISTANCE = 0.5; /**< Distance between ball and placement location at which we no longer include the distance to the edges of the placement location in scoring the trajectory */ +constexpr double OUT_OF_FIELD_MARGIN = 0.17; /**< Distance that the center of the robot is allowed to go out of the field during play (not for end location, only for paths) */ /// Keeper constants constexpr double DISTANCE_FROM_GOAL_CLOSE = 2 * ROBOT_RADIUS; /**< Distance from the keeper to the goal at which the keeper is considered close to that goal */ diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 7d1b37065..dcea37d42 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -63,9 +63,8 @@ std::optional WorldObjects::getFirstDefenseAreaCollision(const rt void WorldObjects::calculateFieldCollisions(const rtt::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, size_t completedTimeSteps) { - double robotRadius = rtt::ai::Constants::ROBOT_RADIUS(); for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { - if (!field.playArea.contains(pathPoints[i], robotRadius)) { + if (!field.playArea.contains(pathPoints[i], rtt::ai::stp::control_constants::OUT_OF_FIELD_MARGIN)) { // Don't care about the field if the robot is already outside the field (i == 0 is the first point of the robot's path, so almost the currentPosition). if (i == 0) return; From a127c0aa084c7bf0fd8b018feeab337970e5c34b Mon Sep 17 00:00:00 2001 From: Jorn Date: Thu, 25 Jan 2024 13:40:57 +0100 Subject: [PATCH 16/17] Remove target location speed, only keep it for harasher --- roboteam_ai/include/roboteam_ai/stp/StpInfo.h | 8 -------- .../src/stp/computations/PositionComputations.cpp | 8 +------- roboteam_ai/src/utilities/Dealer.cpp | 10 ++++++---- 3 files changed, 7 insertions(+), 19 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h index c23990d05..2f30e45cf 100644 --- a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h +++ b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h @@ -47,9 +47,6 @@ struct StpInfo { const std::optional& getPositionToDefend() const { return positionToDefend; } void setPositionToDefend(const std::optional& position) { this->positionToDefend = position; } - const Vector2& getTargetLocationSpeed() const { return targetLocationSpeed; } - void setTargetLocationSpeed(const Vector2& speed) { this->targetLocationSpeed = speed; } - double getKickChipVelocity() const { return kickChipVelocity; } void setKickChipVelocity(double kickChipVelocity) { this->kickChipVelocity = kickChipVelocity; } @@ -137,11 +134,6 @@ struct StpInfo { */ std::optional positionToDefend; - /** - * Speed of the target location (e.g. speed of the robot or speed of the ball) - */ - Vector2 targetLocationSpeed = Vector2(0, 0); - /** * Velocity of the kick/chip */ diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index d26b74b2c..601d3cf29 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -229,7 +229,6 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_map, stp::control_constants::MAX_ROBOT_COUNT> *roles, const Field &field, world::World *world) noexcept { auto enemyClosestToBall = world->getWorld()->getRobotClosestToBall(world::them); - stpInfos["harasser"].setTargetLocationSpeed(world->getWorld()->getBall()->get()->velocity); // 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); @@ -304,7 +303,6 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma defendSpeed.x = 0; } stpInfos["defender_" + std::to_string(i)].setPositionToDefend(defendPostion); - stpInfos["defender_" + std::to_string(i)].setTargetLocationSpeed(defendSpeed); ComputationManager::calculatedEnemyMapIds.emplace_back(enemyMap.begin()->second.id); enemyMap.erase(enemyMap.begin()); } @@ -324,11 +322,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma : enemyMap.begin()->second.position); ComputationManager::calculatedEnemyMapIds.emplace_back(enemyMap.begin()->second.id); for (int j = 0; j < row_length; j++) { - cost_matrix[i][j] = (stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos() + - stpInfos[activeDefenderNames[i]].getRobot()->get()->getVel() * control_constants::DEALER_SPEED_FACTOR) - .dist(enemies[j] + (mustStayOnOurSide && (enemyMap.begin()->second.position.x > 0) ? Vector2{0, enemyMap.begin()->second.velocity.y} - : enemyMap.begin()->second.velocity) * - control_constants::DEALER_SPEED_FACTOR); + cost_matrix[i][j] = stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos().dist(enemies[j]); } enemyMap.erase(enemyMap.begin()); } diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index 42ad33b5b..18efe8a7b 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -262,11 +262,13 @@ double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::Ro return 0; } double dealer_speed_factor = stp::control_constants::DEALER_SPEED_FACTOR; - if ((robot->getPos() - target_position.value()).length() < robot->getVel().length() * dealer_speed_factor) { - dealer_speed_factor = (robot->getPos() - target_position.value()).length() / robot->getVel().length(); + // 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); } - target_position = target_position.value() + stpInfo.getTargetLocationSpeed() * dealer_speed_factor; - distance = (robot->getPos() + robot->getVel() * dealer_speed_factor).dist(*target_position); return costForDistance(distance, field->playArea.width(), field->playArea.height()); } From 03fc79b73ae5f15ae709c160956f33fa25c03397 Mon Sep 17 00:00:00 2001 From: Jorn Date: Thu, 25 Jan 2024 16:03:09 +0100 Subject: [PATCH 17/17] Make positions during kick off and free kick consistent with whatever play happens afterwards --- .../stp/computations/PositionComputations.cpp | 2 +- .../referee_specific/FreeKickUsAtGoal.cpp | 30 +++++++++---------- .../plays/referee_specific/FreeKickUsPass.cpp | 18 +++++------ .../stp/plays/referee_specific/KickOffUs.cpp | 2 +- .../referee_specific/KickOffUsPrepare.cpp | 19 ++++++------ 5 files changed, 35 insertions(+), 36 deletions(-) diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 601d3cf29..0e0ca66d9 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -449,7 +449,7 @@ void PositionComputations::calculateInfoForFormationOurSide(std::unordered_map("keeper"), std::make_unique("free_kick_taker"), - std::make_unique("attacker_0"), std::make_unique("defender_0"), std::make_unique("defender_1"), - std::make_unique("attacker_1"), - // Additional roles if we play 11v11 std::make_unique("waller_0"), - std::make_unique("waller_1"), std::make_unique("defender_2"), - std::make_unique("waller_2"), - std::make_unique("attacker_2"), + // Additional roles if we play 11v11 + std::make_unique("defender_3"), + std::make_unique("attacker_0"), + std::make_unique("waller_1"), + std::make_unique("defender_4"), + std::make_unique("defender_5"), }; } @@ -54,15 +54,15 @@ Dealer::FlagMap FreeKickUsAtGoal::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"free_kick_taker", {DealerFlagPriority::REQUIRED, {kickerFlag, detectionFlag}}}); - flagMap.insert({"waller_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"waller_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"attacker_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"attacker_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"attacker_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_3", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_4", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_5", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"attacker_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); return flagMap; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp index 4c06c2498..665018b02 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp @@ -31,14 +31,14 @@ FreeKickUsPass::FreeKickUsPass() : Play() { std::make_unique("free_kick_taker"), std::make_unique("receiver"), std::make_unique("defender_0"), - std::make_unique("attacker_0"), std::make_unique("defender_1"), + std::make_unique("attacker_0"), // Additional roles if we play 11v11 std::make_unique("waller_0"), std::make_unique("waller_1"), - std::make_unique("defender_2"), - std::make_unique("waller_2"), std::make_unique("attacker_1"), + std::make_unique("defender_2"), + std::make_unique("attacker_2"), }; } @@ -56,14 +56,14 @@ Dealer::FlagMap FreeKickUsPass::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {}, passInfo.keeperId}}); flagMap.insert({"free_kick_taker", {DealerFlagPriority::REQUIRED, {}, passInfo.passerId}}); flagMap.insert({"receiver", {DealerFlagPriority::REQUIRED, {}, passInfo.receiverId}}); - flagMap.insert({"waller_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"waller_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"waller_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"waller_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"attacker_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"attacker_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"attacker_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); return flagMap; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp index 141cd7969..01497ed11 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp @@ -63,7 +63,7 @@ Dealer::FlagMap KickOffUs::decideRoleFlags() const noexcept { void KickOffUs::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForKeeper(stpInfos, field, world); // Kicker - Vector2 passLocation = Vector2(-1.0, 1.0); + Vector2 passLocation = Vector2(-field.playArea.width() / 5, 0); stpInfos["kick_off_taker"].setPositionToShootAt(passLocation); stpInfos["kick_off_taker"].setShotType(ShotType::PASS); stpInfos["kick_off_taker"].setKickOrChip(KickOrChip::KICK); diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp index dcda17d67..d0ba0ae0d 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp @@ -24,16 +24,15 @@ KickOffUsPrepare::KickOffUsPrepare() : Play() { std::make_unique("keeper"), std::make_unique("kicker"), std::make_unique("formation_mid_0"), - std::make_unique("formation_back_0"), std::make_unique("formation_front_0"), - std::make_unique("formation_mid_1"), - // Additional roles if we play 11v11 std::make_unique("formation_front_1"), - std::make_unique("formation_back_1"), - std::make_unique("formation_mid_2"), std::make_unique("formation_front_2"), - std::make_unique("formation_back_2"), - + // Additional roles if we play 11v11 + std::make_unique("formation_back_0"), + std::make_unique("formation_back_1"), + std::make_unique("formation_front_3"), + std::make_unique("formation_front_4"), + std::make_unique("formation_front_5"), }; } @@ -53,13 +52,13 @@ Dealer::FlagMap KickOffUsPrepare::decideRoleFlags() const noexcept { flagMap.insert({"kicker", {DealerFlagPriority::REQUIRED, {kickerFlag, detectionFlag}}}); flagMap.insert({"formation_back_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"formation_back_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"formation_back_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"formation_mid_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_mid_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_mid_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"formation_front_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"formation_front_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"formation_front_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_3", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_4", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_5", {DealerFlagPriority::HIGH_PRIORITY, {}}}); return flagMap; }