Merge pull request #118 from RoboTeamTwente/Feature/project-target-ou…

…tside-of-robots

Feature/project target outside of robots

roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h

@@ -105,6 +105,26 @@ class PositionControl {
     rtt::BB::CommandCollision computeAndTrackTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity,
                                                         Vector2 targetPosition, double maxRobotVelocity, stp::PIDType pidType, stp::AvoidObjects avoidObjects);
 
+    /**
+     * @brief Handles the collision with the ball at the current position. This function will calculate a new target, moving away from the ball as quickly as possible.
+     * @param world a pointer to the current world
+     * @param field the field object, used onwards by the collision detector
+     * @param currentPosition the current position of the robot
+     * @param avoidObjects whether or not to avoid objects
+     * @return A new trajectory to the target
+     */
+    Vector2 handleBallCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects);
+
+    /**
+     * @brief Handles the collision with the ball placement at the current position, will move away from the ball placement location to ball line as quickly as possible.
+     * @param world a pointer to the current world
+     * @param field the field object, used onwards by the collision detector
+     * @param currentPosition the current position of the robot
+     * @param avoidObjects whether or not to avoid objects
+     * @return A new trajectory to the target
+     */
+    Vector2 handleBallPlacementCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects);
+
     /**
      * @brief Calculates a score for a trajectory. The score is based on the distance to the target and the
      * distance to the first collision on the path

roboteam_ai/include/roboteam_ai/stp/Play.hpp

@@ -156,6 +156,12 @@ class Play {
      */
     void reassignRobots() noexcept;
 
+    /**
+     * @brief Draws the margins for the defence area, ball and cardRobot.
+     * This function will draw margins in the interfacce
+     */
+    void DrawMargins() noexcept;
+
     size_t previousRobotNum{}; /**< The previous amount of robots. This is used to check if we need to re-deal (if a robot disappears for example) */
 
     int previousKeeperId = -1;  /**< The previous keeperId. This is used to check if we need to re-deal (if keeper id was changed from UI or GameController) */

roboteam_ai/include/roboteam_ai/stp/Role.hpp

@@ -59,6 +59,11 @@ class Role {
      */
     void forceNextTactic() noexcept;
 
+    /**
+     * @brief Forces the Role to skip to the last tactic in the state machine
+     */
+    void forceLastTactic() noexcept;
+
     /**
      * @brief Resets the tactics, skills and robot of this role so re-dealing of robots works as expected
      */

roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h

@@ -31,6 +31,11 @@ struct EnemyInfo {
     int id;
 };
 
+struct HarasserInfo {
+    int harasserId;
+    double timeToBall;
+};
+
 /**
  * @brief class with computations about positions
  */
@@ -70,13 +75,15 @@ class PositionComputations {
     static Vector2 getWallPosition(int index, int amountDefenders, const Field &field, world::World *world);
 
     /**
-     * @brief Calculates where a robot should stand to prevent the ball from going in the goal
-     * @param field The current field
+     * @brief Calculates the position of the robot to avoid other robots
+     * @param targetPosition The initial target position
      * @param world The current world
-     * @return The position a robot should go to to block the ball (this does not depend on the position of any of our robots)
+     * @param robotId The id of the robot
+     * @param avoidObj The objects to avoid
+     * @param field The current field
+     * @return A position that is not too close to other robots
      */
-    static Vector2 getBallBlockPosition(const Field &field, const world::World *world);
-
+    static Vector2 calculateAvoidRobotsPosition(Vector2 targetPosition, const world::World *world, int robotId, const AvoidObjects &avoidObj, const Field &field);
     /**
      * @brief Calculates a position, near the target position, that is not too close to the ball
      * @param targetPosition The initial target position
@@ -94,15 +101,24 @@ class PositionComputations {
      */
     static void calculateInfoForKeeper(std::unordered_map<std::string, StpInfo> &stpInfos, const Field &field, world::World *world) noexcept;
 
+    /**
+     * @brief Calculates the id of the harasser
+     * @param world The current world
+     * @param field The current field
+     * @return HarasserInfo with the id and the time to the ball
+     */
+    static HarasserInfo calculateHarasserId(rtt::world::World *world, const Field &field) noexcept;
+
     /**
      * @brief Calculates info for the harasser role
      * @param stpInfos The current stpInfos
      * @param roles The current roles
      * @param field The current field
      * @param world The current world
+     * @param timeToBall The time to the ball
      */
     static void calculateInfoForHarasser(std::unordered_map<std::string, StpInfo> &stpInfos, std::array<std::unique_ptr<Role>, stp::control_constants::MAX_ROBOT_COUNT> *roles,
-                                         const Field &field, world::World *world) noexcept;
+                                         const Field &field, world::World *world, double timeToBall) noexcept;
 
     /**
      * @brief Calculates info for the defenders

roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h

@@ -19,8 +19,8 @@ constexpr double MAX_KICK_ATTEMPTS = 25; /**< Maximum allowed kicking attempts *
 constexpr double MAX_CHIP_ATTEMPTS = 25; /**< Maximum allowed chipping attempts */
 
 /// Robot physical constants
-constexpr double ROBOT_RADIUS = 0.088;   /**< Radius of a robot */
-constexpr double CENTER_TO_FRONT = 0.05; /**< Distance from center of the robot to the front of the robot */
+constexpr double ROBOT_RADIUS = 0.088;    /**< Radius of a robot */
+constexpr double CENTER_TO_FRONT = 0.069; /**< Distance from center of the robot to the front of the robot */
 
 /// Dribbler constants
 // The distance from robot to ball at which the dribbler should turn on
@@ -52,9 +52,8 @@ constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot
 // Angle margin for 'goToPos'. If the robot is within this margin, goToPos is successful
 constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.04; /**< Angle error for a robot to be considered to have reached a position */
 // Maximum inaccuracy during ballplacement
-constexpr double BALL_PLACEMENT_MARGIN = 0.15;            /**< Distance error for the ball to be considered to have reached the ball placement position*/
-constexpr double DEALER_SPEED_FACTOR = 0.5;               /**< Multiplication factor of speed used by the dealer */
-constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9; /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */
+constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS; /**< Distance error for the ball to be considered to have reached the ball placement position*/
+constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9;    /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */
 
 /// Invariant constants
 constexpr uint8_t FUZZY_TRUE = 255;          /**< Value at which the fuzzy logic is considered 100% true */
@@ -81,6 +80,10 @@ constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARG
 constexpr double AVOID_BALL_DISTANCE_BEFORE_FREE_KICK =
     0.05 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.01; /**< Minimum distance all robots should keep when avoiding the ball before a free kick */
 
+/// Friction constants
+constexpr static float SIMULATION_FRICTION = 0.71; /**< The expected movement friction of the ball during simulation */
+constexpr static float REAL_FRICTION = 0.44;       /**< The expected movement friction of the ball on the field */
+
 }  // namespace rtt::ai::stp::control_constants
 
 #endif  // RTT_CONTROLCONSTANTS_H

roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h

@@ -44,6 +44,8 @@ class DefendPass : public Play {
      * @return The name of the play as string
      */
     const char* getName() const override;
+
+    HarasserInfo harasserInfo;
 };
 }  // namespace rtt::ai::stp::play
 

roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h

@@ -43,6 +43,8 @@ class DefendShot : public Play {
      * @brief Gets the play name
      */
     const char* getName() const override;
+
+    HarasserInfo harasserInfo;
 };
 }  // namespace rtt::ai::stp::play
 

roboteam_ai/include/roboteam_ai/stp/roles/active/PenaltyTaker.h

@@ -14,7 +14,14 @@ class PenaltyTaker : public Role {
      * @param name The name of the role
      */
     PenaltyTaker(std::string name);
+
+    /**
+     * @brief Method that updates the role
+     * @param info The information to update the role with
+     * @return Status of the update
+     */
+    [[nodiscard]] Status update(StpInfo const& info) noexcept override;
 };
 }  // namespace rtt::ai::stp::role
 
-#endif  // RTT_PENALTYTAKER_H
+#endif  // RTT_PENALTYTAKER_H

roboteam_ai/include/roboteam_ai/stp/tactics/passive/BallStandBack.h

@@ -55,6 +55,8 @@ class BallStandBack : public Tactic {
      * @return The name of this tactic
      */
     const char *getName() override;
+
+    bool standBack;
 };
 }  // namespace rtt::ai::stp::tactic
 

roboteam_ai/include/roboteam_ai/utilities/Dealer.h

@@ -23,7 +23,7 @@ namespace v = rtt::world::view;
 /**
  * @brief Enumerator that defines the name of the dealerFlags
  */
-enum class DealerFlagTitle { WITH_WORKING_BALL_SENSOR, WITH_WORKING_DRIBBLER, READY_TO_INTERCEPT_GOAL_SHOT, KEEPER, CAN_DETECT_BALL, CAN_KICK_BALL };
+enum class DealerFlagTitle { WITH_WORKING_BALL_SENSOR, WITH_WORKING_DRIBBLER, KEEPER, CAN_DETECT_BALL, CAN_KICK_BALL };
 
 /**
  * @brief Enumerator that defines the priority of the dealerFlags

roboteam_ai/include/roboteam_ai/world/Ball.hpp

@@ -20,9 +20,10 @@ namespace rtt::world::ball {
  */
 class Ball {
    public:
-    Vector2 position;     /**< Position of the ball */
-    Vector2 velocity;     /**< Velocity of the ball */
-    bool visible = false; /**< Whether the ball is visible by any camera */
+    Vector2 position;            /**< Position of the ball */
+    Vector2 velocity;            /**< Velocity of the ball */
+    Vector2 expectedEndPosition; /**< Expected end position of the ball if it keeps moving in the same direction */
+    bool visible = false;        /**< Whether the ball is visible by any camera */
 
     /**
      * @brief Initializes ball at the previously seen position, if the current ball is not visible

roboteam_ai/include/roboteam_ai/world/FieldComputations.h

@@ -44,6 +44,13 @@ class FieldComputations {
      */
     static bool getBallAvoidance();
 
+    /**
+     * @brief Get the expected ball position after a certain amount of time
+     * @param ball The current ball position
+     * @param time The time in seconds after which the expected ball position should be returned
+     */
+    static Vector2 getBallPositionAtTime(const rtt::world::ball::Ball &ball, double time);
+
     /**
      * @brief Check whether a given point is a valid position given which parts of the field should be avoided (note that shouldAvoidBall is not taken into consideration)
      * @param field The field class which is used to determine the boundaries of the field.

roboteam_ai/src/control/ControlModule.cpp

@@ -84,9 +84,9 @@ void ControlModule::simulator_angular_control(const std::optional<::rtt::world::
         } else {
             // initialize PID controller for robot
             // below tuning only works ish for erforce, is completely useless in grsim
-            double P = 4.0;
+            double P = 2.5;
             double I = 0.0;
-            double D = 0.01;
+            double D = 0;
             double max_ang_vel = 5.0;  // rad/s
             double dt = 1. / double(Constants::STP_TICK_RATE());
 

roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp

@@ -27,6 +27,9 @@ std::optional<CollisionData> WorldObjects::getFirstCollision(const rtt::world::W
     if (avoidObjects.shouldAvoidDefenseArea) {
         calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]);
     }
+    if (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM) {
+        calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]);
+    }
     if (avoidObjects.shouldAvoidBall) {
         calculateBallCollisions(world, collisionDatas, pathPoints, timeStep, avoidObjects.avoidBallDist, completedTimeSteps[robotId], startTime);
     }
@@ -36,9 +39,6 @@ std::optional<CollisionData> WorldObjects::getFirstCollision(const rtt::world::W
     if (avoidObjects.shouldAvoidOurRobots) {
         calculateOurRobotCollisions(world, collisionDatas, pathPoints, computedPaths, robotId, timeStep, completedTimeSteps, startTime);
     }
-    if (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM) {
-        calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]);
-    }
     if (!collisionDatas.empty()) {
         return collisionDatas[0];
     } else {

roboteam_ai/src/control/positionControl/PositionControl.cpp

@@ -60,6 +60,15 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt::
             firstCollision = worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects, completedTimeSteps);
             if (firstCollision.has_value()) {
                 commandCollision.collisionData = firstCollision;
+
+                if ((firstCollision.value().collisionType == BB::CollisionType::BALL) && firstCollision.value().collisionTime <= 0.11) {
+                    targetPosition = handleBallCollision(world, field, currentPosition, avoidObjects);
+                    computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, 1.0, ai::Constants::MAX_ACC_UPPER());
+                }
+                if ((firstCollision.value().collisionType == BB::CollisionType::BALLPLACEMENT) && firstCollision.value().collisionTime <= 0.11) {
+                    targetPosition = handleBallPlacementCollision(world, field, currentPosition, avoidObjects);
+                    computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, 1.0, ai::Constants::MAX_ACC_UPPER());
+                }
             }
         }
 
@@ -116,6 +125,57 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt::
     return commandCollision;
 }
 
+rtt::Vector2 PositionControl::handleBallCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects) {
+    auto ballPos = world->getWorld()->getBall()->get()->position;
+    auto direction = currentPosition - ballPos;
+    Vector2 targetPosition = currentPosition + direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2);
+    if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) {
+        return targetPosition;
+    }
+    int rotationStepDegrees = 10;
+    int maxRotationDegrees = 90;
+    for (int i = rotationStepDegrees; i <= maxRotationDegrees; i += rotationStepDegrees) {
+        for (int sign : {1, -1}) {
+            double rotation = sign * i * M_PI / 180;
+            Vector2 rotatedDirection = direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2).rotate(rotation);
+            Vector2 potentialTargetPosition = currentPosition + rotatedDirection;
+            if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) {
+                return potentialTargetPosition;
+            }
+        }
+    }
+    return currentPosition + direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2).rotate(M_PI / 2);
+}
+
+rtt::Vector2 PositionControl::handleBallPlacementCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects) {
+    auto placementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition();
+    auto ballPos = world->getWorld()->getBall()->get()->position;
+    auto direction = (placementPos - ballPos).stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2);
+    double distance = (placementPos - ballPos).length();
+    if (distance > 0.10) {  // distance is more than 10cm
+        direction = direction.rotate((currentPosition - ballPos).cross(placementPos - ballPos) < 0 ? M_PI / 2 : -M_PI / 2);
+    } else {  // distance is less than or equal to 10cm
+        direction = (currentPosition - ballPos).stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2);
+    }
+    Vector2 targetPosition = currentPosition + direction;
+    if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) {
+        return targetPosition;
+    }
+    int rotationStepDegrees = 10;
+    int maxRotationDegrees = 90;
+    for (int i = rotationStepDegrees; i <= maxRotationDegrees; i += rotationStepDegrees) {
+        for (int sign : {1, -1}) {
+            double rotation = sign * i * M_PI / 180;
+            Vector2 rotatedDirection = direction.rotate(rotation);
+            Vector2 potentialTargetPosition = currentPosition + rotatedDirection;
+            if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) {
+                return potentialTargetPosition;
+            }
+        }
+    }
+    return targetPosition;
+}
+
 double PositionControl::calculateScore(const rtt::world::World *world, const rtt::Field &field, std::optional<BB::CollisionData> &firstCollision,
                                        Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects, double startTime) {
     double totalTime = trajectoryAroundCollision.getTotalTime();
@@ -164,7 +224,7 @@ std::optional<Trajectory2D> PositionControl::findNewTrajectory(const rtt::world:
               [&targetPosition](const Vector2 &a, const Vector2 &b) { return (a - targetPosition).length() < (b - targetPosition).length(); });
     timeStep *= 3;
 
-    double bestScore = 999;
+    double bestScore = std::numeric_limits<double>::max();
     std::optional<Trajectory2D> bestTrajectory = std::nullopt;
 
     for (const auto &intermediatePoint : intermediatePoints) {
@@ -198,15 +258,17 @@ std::vector<Vector2> PositionControl::createIntermediatePoints(const rtt::Field
     float angleBetweenIntermediatePoints = M_PI_4 / 2;
     float fieldHeight = field.playArea.height();
     float pointExtension = fieldHeight / 18;
-    Vector2 collisionToTargetNormalized = (firstCollision->collisionPosition - targetPosition).normalize();
+    Vector2 collisionToTargetNormalized = (targetPosition - firstCollision->collisionPosition).normalize();
     Vector2 pointToDrawFrom = firstCollision->collisionPosition + collisionToTargetNormalized * pointExtension;
 
     std::vector<Vector2> intermediatePoints;
     float intermediatePointRadius = fieldHeight / 4;
     Vector2 pointToRotate = pointToDrawFrom + collisionToTargetNormalized * intermediatePointRadius;
     for (int i = -6; i < 7; i++) {
         if (i != 0) {
-            Vector2 intermediatePoint = pointToRotate.rotateAroundPoint(i * angleBetweenIntermediatePoints, pointToDrawFrom);
+            // The slight offset to the angle makes sure the points are not symmetrically placed, this means we don't keep on switching between two points that are equally good
+            // when there is a collision at time 0ss
+            Vector2 intermediatePoint = pointToRotate.rotateAroundPoint(i * angleBetweenIntermediatePoints - 0.01, pointToDrawFrom);
 
             if (field.playArea.contains(intermediatePoint)) {
                 intermediatePoints.emplace_back(intermediatePoint);