Merge pull request #134 from RoboTeamTwente/interceptImprovements

Intercept improvements

docker/runner/docker-compose.yml

@@ -18,7 +18,7 @@ services:
       - LD_LIBRARY_PATH=/home/roboteamtwente/lib/
     volumes:
       - ../../build/release/:/home/roboteamtwente/
-    profiles: ["simulator","diff","game"]
+    profiles: ["simulator","diff","game", "robocup"]
 
   roboteam_secondary_ai:
     image: roboteamtwente/roboteam:development
@@ -61,7 +61,7 @@ services:
     container_name: RTT_roboteam_observer_sim
     restart: always
     working_dir: "/home/roboteamtwente/"
-    command: sh -c "./bin/roboteam_observer --vision-ip 224.5.23.2 --referee-ip 224.5.23.1 --vision-port 10020 --referee-port 10003"
+    command: sh -c "./bin/roboteam_observer --vision-ip 224.5.23.2 --referee-ip 224.5.23.1 --vision-port 10020 --referee-port 10003 --log"
     network_mode: "host" # Workaround to connect from interface on host to AI websocket, please fix
     expose:
       - 1-65535:1-65535 # For zeromq ports
@@ -79,7 +79,7 @@ services:
     container_name: RTT_roboteam_observer_game
     restart: always
     working_dir: "/home/roboteamtwente/"
-    command: sh -c "./bin/roboteam_observer --vision-ip 224.5.23.2 --referee-ip 224.5.23.1 --vision-port 10006 --referee-port 10003"
+    command: sh -c "./bin/roboteam_observer --vision-ip 224.5.23.2 --referee-ip 224.5.23.1 --vision-port 10006 --referee-port 10003 --log"
     network_mode: "host" # Workaround to connect from interface on host to AI websocket, please fix
     expose:
       - 1-65535:1-65535 # For zeromq ports
@@ -90,7 +90,7 @@ services:
       - LD_LIBRARY_PATH=/home/roboteamtwente/lib/
     volumes:
       - ../../build/release/:/home/roboteamtwente/
-    profiles: ["game","autoref"]
+    profiles: ["game","autoref", "robocup"]
 
   roboteam_robothub_sim:
     image: roboteamtwente/roboteam:development
@@ -135,7 +135,7 @@ services:
     privileged: true
     devices:
       - /dev/rtt-basestation:/dev/ttyACM1
-    profiles: ["game"]
+    profiles: ["game", "robocup"]
 
   roboteam_interface:
     image: roboteamtwente/roboteam:development
@@ -147,7 +147,7 @@ services:
       - 8080:8080
     volumes:
       - ../../roboteam_interface/:/home/roboteamtwente/
-    profiles: ["simulator","diff","game"]
+    profiles: ["simulator","diff","game", "robocup"]
 
   roboteam_autoref:
     image: gradle:8.4.0-jdk17

roboteam_ai/include/roboteam_ai/utilities/Constants.h

@@ -44,11 +44,9 @@ constexpr double MAX_VEL_WHEN_HAS_BALL = 3.0; /**< Maximum allowed velocity that
 constexpr double MAX_ANGULAR_VELOCITY = 6.0;  /**< Maximum allowed angular velocity that can be send to the robot */
 constexpr double MIN_YAW = -M_PI;             /**< Minimum yaw the robot can have */
 constexpr double MAX_YAW = M_PI;              /**< Maximum yaw the robot can have */
-constexpr double MAX_ACC = 3.5;               /**< Maximum acceleration of the robot */
-constexpr double MAX_VEL = 4.0;               /**< Maximum allowed velocity of the robot */
-constexpr double MAX_JERK_OVERSHOOT = 100;    /**< Jerk limit for overshoot */
-// TODO ROBOCUP 2024: FIX THIS MAGIC
-constexpr double MAX_JERK_DEFAULT = 6; /**< Default jerk limit */
+constexpr double MAX_ACC = 2.0;               /**< Maximum acceleration of the robot */
+constexpr double MAX_VEL = 2.0;               /**< Maximum allowed velocity of the robot */
+constexpr double MAX_JERK_DEFAULT = 6;        /**< Default jerk limit */
 
 /// GoToPos Constants
 // Distance margin for 'goToPos'. If the robot is within this margin, goToPos is successful
@@ -57,7 +55,7 @@ constexpr double GO_TO_POS_ERROR_MARGIN = 0.02; /**< Distance error for a robot
 constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.01; /**< Angle error for a robot to be considered to have reached a position */
 // Maximum inaccuracy during ballplacement
 constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS - 0.01; /**< Distance error for the ball to be considered to have reached the ball placement position*/
-constexpr double BALL_PLACER_MARGIN = BALL_PLACEMENT_MARGIN - 0.05; /**< Distance before the ball placer moves away from hte ball*/
+constexpr double BALL_PLACER_MARGIN = BALL_PLACEMENT_MARGIN - 0.07; /**< Distance before the ball placer moves away from hte ball*/
 constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9;           /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */
 
 /// Invariant constants
@@ -80,7 +78,7 @@ constexpr double AVOID_BALL_DISTANCE_BEFORE_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 */
+constexpr static float REAL_FRICTION = 0.526;      /**< The expected movement friction of the ball on the field */
 
 static inline double HAS_BALL_DISTANCE() { return (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? 0.11 : 0.12; }
 static inline double SEND_TIME_IN_FUTURE() {

roboteam_ai/include/roboteam_ai/utilities/RuleSet.h

@@ -49,9 +49,9 @@ struct RuleSet {
         }
     }
 
-    static constexpr RuleSet RULESET_DEFAULT() { return {RuleSetName::DEFAULT, 4.0}; }
+    static constexpr RuleSet RULESET_DEFAULT() { return {RuleSetName::DEFAULT, 2.0}; }
     static constexpr RuleSet RULESET_HALT() { return {RuleSetName::HALT, 0.0}; }
-    static constexpr RuleSet RULESET_STOP() { return {RuleSetName::STOP, 1.3}; }
+    static constexpr RuleSet RULESET_STOP() { return {RuleSetName::STOP, 1.0}; }
 
     static constexpr std::array<RuleSet, 3> ruleSets() {
         return {

roboteam_ai/src/control/ControlUtils.cpp

@@ -23,9 +23,9 @@ double ControlUtils::determineKickForce(const double distance, stp::ShotPower sh
 
     double kickForce;
     if (shotPower == stp::ShotPower::PASS) {
-        kickForce = 0.8;
+        kickForce = 1.8;
     } else if (shotPower == stp::ShotPower::TARGET) {
-        kickForce = 0.5;
+        kickForce = 1.2;
     } else {
         RTT_WARNING("No shotPower set! Setting kickForce to 0")
         kickForce = 0;

roboteam_ai/src/control/positionControl/CollisionCalculations.cpp

@@ -9,8 +9,8 @@
 namespace rtt::ai::control {
 
 double CollisionCalculations::getFirstCollisionTimeMotionlessObject(const Trajectory2D &Trajectory, stp::AvoidObjects avoidObjects, const Field &field) {
-    auto pathPoints = Trajectory.getPathApproach(0.1);
-    auto maxCheckPoints = std::min(pathPoints.size(), static_cast<size_t>(7));
+    auto pathPoints = Trajectory.getPathApproach(0.05);
+    auto maxCheckPoints = std::min(pathPoints.size(), static_cast<size_t>(14));
 
     auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = FieldComputations::getDefenseAreaMargin();
     const auto &ourDefenseArea = FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1);
@@ -36,17 +36,17 @@ double CollisionCalculations::getFirstCollisionTimeMotionlessObject(const Trajec
         }
         if (avoidObjects.shouldAvoidOurDefenseArea) {
             if (ourDefenseArea.contains(pathPoints[checkPoint]) || ourDefenseArea.contains(pathPoints[checkPoint - 1]) || ourDefenseArea.doesIntersect(pathLine)) {
-                return checkPoint * 0.1;
+                return checkPoint * 0.05;
             }
         }
         if (avoidObjects.shouldAvoidTheirDefenseArea && theirDefenseAreaMargin > constants::ROBOT_RADIUS + constants::GO_TO_POS_ERROR_MARGIN) {
             if (theirDefenseArea.contains(pathPoints[checkPoint]) || theirDefenseArea.contains(pathPoints[checkPoint - 1]) || theirDefenseArea.doesIntersect(pathLine)) {
-                return checkPoint * 0.1;
+                return checkPoint * 0.05;
             }
         }
         if (avoidObjects.shouldAvoidOutOfField) {
             if (!field.playArea.contains(pathPoints[checkPoint], constants::OUT_OF_FIELD_MARGIN)) {
-                return checkPoint * 0.1;
+                return checkPoint * 0.05;
             }
         }
     }
@@ -60,7 +60,7 @@ bool CollisionCalculations::isCollidingWithMotionlessObject(const Trajectory2D &
 
 double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory2D &Trajectory, stp::AvoidObjects avoidObjects, int &robotId, const world::World *world,
                                                                 const std::unordered_map<int, std::vector<Vector2>> &computedPaths) {
-    auto pathPoints = Trajectory.getPathApproach(0.1);
+    auto pathPoints = Trajectory.getPathApproach(0.05);
     auto maxCheckPoints = std::min(pathPoints.size(), static_cast<size_t>(7));
 
     const std::vector<world::view::RobotView> &theirRobots = world->getWorld()->getThem();
@@ -70,21 +70,22 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory
 
     for (int checkPoint = 1; checkPoint < static_cast<int>(maxCheckPoints); checkPoint += 1) {
         auto pathLine = LineSegment(pathPoints[checkPoint - 1], pathPoints[checkPoint]);
-        double velocityOurRobot = Trajectory.getVelocity(checkPoint * 0.1).length();
-        auto positionOurRobot = Trajectory.getPosition(checkPoint * 0.1);
-        double additionalMargin = std::pow(std::min(maxVel, velocityOurRobot) / maxVel, 2) * 0.2;
-        if (velocityOurRobot > 0.4 && avoidObjects.shouldAvoidOurRobots) {
+        double velocityOurRobot = Trajectory.getVelocity(checkPoint * 0.05).length();
+        auto positionOurRobot = Trajectory.getPosition(checkPoint * 0.05);
+        double additionalMargin = std::pow(std::min(3.5, velocityOurRobot) / 3.5, 2) * 0.2;
+        if (velocityOurRobot > 0.7 && avoidObjects.shouldAvoidOurRobots) {
             for (const auto &ourOtherRobot : ourRobots) {
                 const int &ourOtherRobotId = ourOtherRobot->getId();
                 if (ourOtherRobotId == robotId) {
                     continue;
                 }
+
                 const auto computedPathsIt = computedPaths.find(ourOtherRobotId);
                 // If the path of the other robot is not computed, we assume it is not moving
                 if (computedPathsIt == computedPaths.end()) {
                     const Vector2 &ourOtherRobotPos = ourOtherRobot->getPos();
                     if ((ourOtherRobotPos - positionOurRobot).length() < 2 * constants::ROBOT_RADIUS + additionalMargin) {
-                        return checkPoint * 0.1;
+                        return checkPoint * 0.05;
                     }
                 } else {
                     LineSegment pathLineOtherRobot;
@@ -94,44 +95,60 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory
                         pathLineOtherRobot = LineSegment(computedPathsIt->second.back(), computedPathsIt->second.back());
                     }
                     if (pathLineOtherRobot.closestDistanceToLineSegment(pathLine) < 2 * constants::ROBOT_RADIUS + additionalMargin) {
-                        return checkPoint * 0.1;
+                        return checkPoint * 0.05;
                     }
                 }
             }
+        } else if (avoidObjects.shouldAvoidOurRobots) {
+            for (const auto &ourOtherRobot : ourRobots) {
+                const int &ourOtherRobotId = ourOtherRobot->getId();
+                if (ourOtherRobotId == robotId) {
+                    continue;
+                }
+                const Vector2 &ourOtherRobotPos = ourOtherRobot->getPos();
+                if ((ourOtherRobotPos - positionOurRobot).length() < 1.4 * constants::ROBOT_RADIUS) {
+                    return checkPoint * 0.05;
+                }
+            }
         }
         if (velocityOurRobot > 0.7 && avoidObjects.shouldAvoidTheirRobots) {
             if (std::any_of(theirRobots.begin(), theirRobots.end(), [&](const auto &theirRobot) {
-                    return LineSegment(theirRobot->getPos() + theirRobot->getVel() * 0.1 * (checkPoint - 1), theirRobot->getPos() + theirRobot->getVel() * 0.1 * checkPoint)
+                    return LineSegment(theirRobot->getPos() + theirRobot->getVel() * 0.05 * (checkPoint - 1), theirRobot->getPos() + theirRobot->getVel() * 0.05 * checkPoint)
                                .closestDistanceToLineSegment(pathLine) < 2 * constants::ROBOT_RADIUS + additionalMargin;
                 })) {
-                return checkPoint * 0.1;
+                return checkPoint * 0.05;
+            }
+        } else if (avoidObjects.shouldAvoidTheirRobots) {
+            if (std::any_of(theirRobots.begin(), theirRobots.end(),
+                            [&](const auto &theirRobot) { return (theirRobot->getPos() - positionOurRobot).length() < 1.8 * constants::CENTER_TO_FRONT; })) {
+                return checkPoint * 0.05;
             }
         }
         if (avoidObjects.shouldAvoidBall) {
-            auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.1);
+            auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.05);
             if ((ballPosition - positionOurRobot).length() < constants::AVOID_BALL_DISTANCE + additionalMargin) {
-                return checkPoint * 0.1;
+                return checkPoint * 0.05;
             }
         }
         if (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM ||
             GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) {
             auto ballPlacementPosition = GameStateManager::getRefereeDesignatedPosition();
             bool isBallPlacementCollision = true;
-            for (int i = checkPoint; i < checkPoint + 10; i++) {
-                auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.1);
+            for (int i = checkPoint; i < checkPoint + 20; i++) {
+                auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.05);
                 if (i >= static_cast<int>(pathPoints.size())) {
                     isBallPlacementCollision = false;
                     break;
                 }
-                auto positionOurRobot = Trajectory.getPosition(i * 0.1);
+                auto positionOurRobot = Trajectory.getPosition(i * 0.05);
                 auto ballPlacementLine = LineSegment(ballPlacementPosition, ballPosition);
                 if (ballPlacementLine.distanceToLine(positionOurRobot) > constants::AVOID_BALL_DISTANCE + additionalMargin) {
                     isBallPlacementCollision = false;
                     break;
                 }
             }
             if (isBallPlacementCollision) {
-                return checkPoint * 0.1;
+                return checkPoint * 0.05;
             }
         }
     }