Merge pull request #132 from RoboTeamTwente/jerkLimit

Jerk BB + general improvements

.github/workflows/tests-action.yml

@@ -48,6 +48,18 @@ jobs:
               cd protobuf
               sudo make install
               sudo ldconfig
+      
+      - name: Build & Install Ruckig
+        if: steps.cache-build-restore.outputs.cache-hit != 'true'
+        run: |
+              git clone https://github.com/pantor/ruckig.git ruckig
+              cd ruckig
+              git checkout dae82835ec043a54ed6bc775f0776e1a7ff99124
+              mkdir build
+              cd build
+              cmake .. -DCMAKE_BUILD_TYPE=Release
+              make -j $(nproc)
+              sudo make install
             
       - name: Build tests
         if: steps.cache-build-restore.outputs.cache-hit != 'true'

CMakeLists.txt

@@ -95,6 +95,7 @@ if(UNIX AND NOT APPLE)
 endif()
 
 find_package(Qt5Network REQUIRED)
+find_package(ruckig REQUIRED)
 
 # internal projects
 add_subdirectory(roboteam_networking)

docker/Dockerfile

@@ -36,6 +36,15 @@ RUN apk add --no-cache bash build-base cmake make musl-dev libtool \
     qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev
 RUN npm install -g yarn
 
+WORKDIR /tmp
+RUN git clone https://github.com/pantor/ruckig.git /tmp/ruckig \
+    && cd /tmp/ruckig \
+    && git checkout dae82835ec043a54ed6bc775f0776e1a7ff99124 \
+    && mkdir build && cd build \
+    && cmake -DCMAKE_BUILD_TYPE=Release .. \
+    && make -j$(nproc) \
+    && make install
+
 WORKDIR /root/protobuf
 RUN wget https://github.com/protocolbuffers/protobuf/releases/download/v3.20.3/protobuf-cpp-3.20.3.zip -O protobuf.zip && unzip protobuf.zip && rm protobuf.zip
 WORKDIR /root/protobuf/protobuf-3.20.3
@@ -70,7 +79,17 @@ RUN apk add --no-cache \
     sudo nodejs npm
 RUN npm install -g yarn
 
+WORKDIR /tmp
+RUN git clone https://github.com/pantor/ruckig.git /tmp/ruckig \
+    && cd /tmp/ruckig \
+    && git checkout dae82835ec043a54ed6bc775f0776e1a7ff99124 \
+    && mkdir build && cd build \
+    && cmake -DCMAKE_BUILD_TYPE=Release .. \
+    && make -j$(nproc) \
+    && make install
+
 COPY --from=development --chown=root:root /usr/local/lib/libprotobuf.so.31 /usr/local/lib/libprotobuf.so.31
+COPY --from=development --chown=root:root /usr/local/lib/libruckig.so /usr/local/lib/libruckig.so
 COPY --from=development --chown=root:root /usr/local/bin/protoc /usr/local/bin/protoc
 
 RUN adduser -D $USER && \

roboteam_ai/CMakeLists.txt

@@ -133,10 +133,12 @@ target_include_directories(roboteam_ai_control
         PRIVATE include/roboteam_ai
 )
 target_link_libraries(roboteam_ai_control
+        PRIVATE ruckig
         PRIVATE roboteam_networking
         PRIVATE roboteam_utils
         PUBLIC Qt5::Network
 )
+target_compile_features(roboteam_ai_control PUBLIC cxx_std_17)
 target_compile_options(roboteam_ai_control PRIVATE "${COMPILER_FLAGS}")
 
 ###### INTERFACE #######

roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory2D.h

@@ -1,11 +1,13 @@
 #ifndef RTT_TRAJECTORY2D_H
 #define RTT_TRAJECTORY2D_H
 
+#include <ruckig/ruckig.hpp>
 #include <vector>
 
 #include "Trajectory1D.h"
 #include "roboteam_utils/Vector2.h"
 
+using namespace ruckig;
 namespace rtt {
 
 /**
@@ -19,15 +21,11 @@ class Trajectory2D {
      */
     Trajectory2D() = default;
 
-    /**
-     * @brief Constructor of the Trajectory2D class
-     * @param initialPos Initial position of the robot
-     * @param initialVel Initial velocity of the robot
-     * @param finalPos Target position to got to
-     * @param maxVel Maximum allowed velocity
-     * @param maxAcc Maximum allowed acceleration or deceleration
-     */
-    Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc);
+    Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc, double maxJerk, int robotId);
+    Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, const Vector2 &finalVel, double maxVel, double maxAcc, double maxJerk, int robotId);
+    Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &initialAcc, const Vector2 &finalPos, double maxVel, double maxAcc, double maxJerk);
+    Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &initialAcc, const Vector2 &finalPos, const Vector2 &finalVel, double maxVel, double maxAcc,
+                 double maxJerk);
 
     /**
      * @brief Stores the trajectory
@@ -76,10 +74,16 @@ class Trajectory2D {
      * @return Total time of the trajectory to end point
      */
     [[nodiscard]] double getTotalTime() const;
+    static std::unordered_map<int, Vector2> lastAcceleration; /**< Map of last acceleration for each robot */
+
+    Vector2 getLastAcceleration(int robotId);
 
    private:
-    Trajectory1D x; /**< 1D x component of the 2D Trajectory */
-    Trajectory1D y; /**< 1D y component of the 2D Trajectory */
+    Trajectory<1> x; /**< 1D x component of the 2D Trajectory */
+    std::optional<Trajectory<1>> xAdded;
+    Trajectory<1> y; /**< 1D y component of the 2D Trajectory */
+    std::optional<Trajectory<1>> yAdded;
+    double addedFromTime = std::numeric_limits<double>::infinity();
 };
 
 }  // namespace rtt

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

@@ -40,11 +40,13 @@ class PositionControl {
      * @param currentPosition the current position of the aforementioned robot
      * @param currentVelocity its velocity
      * @param targetPosition the desired position that the robot has to reach
+     * @param targetVelocity the desired velocity that the robot has to reach
      * @param maxRobotVelocity the maximum velocity that the robot is allowed to have
+     * @param maxJerk the maximum jerk that the robot is allowed to have
      * @return A RobotCommand and optional with the location of the first collision on the path
      */
-    Vector2 computeAndTrackTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity,
-                                      Vector2 targetPosition, double maxRobotVelocity, stp::AvoidObjects avoidObjects);
+    std::pair<Vector2, Vector2> computeAndTrackTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity,
+                                                          Vector2 targetPosition, Vector2 targetVelocity, double maxRobotVelocity, double maxJerk, stp::AvoidObjects avoidObjects);
 
     /**
      * @brief Handles the collision with the ball at the current position. This function will calculate a new target, moving away from the ball as quickly as possible.
@@ -90,7 +92,7 @@ class PositionControl {
      * @return An optional with a new path
      */
     Trajectory2D findNewTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 &currentPosition, Vector2 &currentVelocity,
-                                   Vector2 &targetPosition, double maxRobotVelocity, double timeStep, stp::AvoidObjects AvoidObjects);
+                                   Vector2 &targetPosition, Vector2 &targetVelocity, double maxRobotVelocity, double maxJerk, double timeStep, stp::AvoidObjects AvoidObjects);
 
     /**
      * @brief Creates normalized random points, which will be used to create intermediate points

roboteam_ai/include/roboteam_ai/stp/StpInfo.h

@@ -32,6 +32,12 @@ struct StpInfo {
     void setPositionToMoveTo(const std::optional<Vector2>& position) { this->positionToMoveTo = position; }
     void setPositionToMoveTo(const std::optional<gen::ScoredPosition>& scoredPosition) { setPositionToMoveTo(scoredPosition->position); }
 
+    const Vector2& getTargetVelocity() const { return targetVelocity; }
+    void setTargetVelocity(const Vector2& targetVelocity) { this->targetVelocity = targetVelocity; }
+
+    double getMaxJerk() const { return maxJerk; }
+    void setMaxJerk(double maxJerk) { this->maxJerk = maxJerk; }
+
     const std::optional<Vector2>& getPositionToShootAt() const { return positionToShootAt; }
     void setPositionToShootAt(const std::optional<Vector2>& position) { this->positionToShootAt = position; }
 
@@ -50,6 +56,9 @@ struct StpInfo {
     ShotPower getShotPower() const { return shotPower; }
     void setShotPower(ShotPower shotPower) { this->shotPower = shotPower; }
 
+    bool getShootOnFirstTouch() const { return shootOnFirstTouch; }
+    void setShootOnFirstTouch(bool shootOnFirstTouch) { this->shootOnFirstTouch = shootOnFirstTouch; }
+
     rtt::KickType getKickOrChip() const { return kickOrChip; }
     void setKickOrChip(rtt::KickType kickOrChip) { this->kickOrChip = kickOrChip; }
 
@@ -133,6 +142,16 @@ struct StpInfo {
      */
     std::optional<Vector2> positionToMoveTo;
 
+    /**
+     * Target velocity to move with
+     */
+    Vector2 targetVelocity;
+
+    /**
+     * Jerk to move with
+     */
+    double maxJerk = ai::constants::MAX_JERK_DEFAULT;
+
     /**
      * Position to kick or chip at
      */
@@ -153,6 +172,11 @@ struct StpInfo {
      */
     ShotPower shotPower{};
 
+    /**
+     * Shoot as soon as we see the ball, also when we are not in a kick skill
+     */
+    bool shootOnFirstTouch = false;
+
     /**
      * Reference yaw of the robot
      */

roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h

@@ -21,6 +21,7 @@ namespace rtt::ai::stp {
 
 struct InterceptionInfo {
     Vector2 interceptLocation;
+    Vector2 interceptVelocity;
     int interceptId = -1;
     double timeToIntercept = std::numeric_limits<double>::max();
     bool isInterceptable = false;

roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h

@@ -55,34 +55,40 @@ class PassComputations {
      * @param point the point to check for validity
      * @param possibleReceiverLocations the locations of all robots that could receive the ball
      * @param possibleReceiverVelocities the velocities of all robots that could receive the ball
+     * @param possiblePassLocations the locations of all robots that could pass the ball
      * @param passLocation the location where the ball will be passed from
      * @param passerLocation the location of the robot that will pass the ball
      * @param passerVelocity the velocity of the robot that will pass the ball
+     * @param passerId the id of the robot that will pass the ball
      * @param field the current field
      * @param world the current world
      * @return bool indicating whether this point is (likely) possible to pass to
      */
+
     static bool pointIsValidReceiverLocation(Vector2 point, const std::vector<Vector2>& possibleReceiverLocations, const std::vector<Vector2>& possibleReceiverVelocities,
-                                             Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, const Field& field, const world::World* world);
+                                             const std::vector<int>& possibleReceiverIds, Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, int passerId,
+                                             const Field& field, const world::World* world);
 
     /**
      * @brief Approximate the time it takes a robot to reach a point
      * @param robotPosition current position of robot
      * @param robotVelocity current velocity of robot
+     * @param robotId id of robot
      * @param targetPosition position to calculate travel time to
      * @return approximated time to reach target from position
      */
-    static double calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, Vector2 targetPosition);
+    static double calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, int robotId, Vector2 targetPosition);
 
     /**
      * Approximate the time it takes the ball to reach a point
      * @param passLocation the location where the ball will be passed from
      * @param passerLocation current location of the passer
+     * @param passerId id of the passer
      * @param robotVelocity current velocity of robot
      * @param targetPosition position to calculate travel time to
      * @return approximated time for ball to reach target position
      */
-    static double calculateBallTravelTime(Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, Vector2 targetPosition);
+    static double calculateBallTravelTime(Vector2 passLocation, Vector2 passerLocation, int passerId, Vector2 passerVelocity, Vector2 targetPosition);
 };
 }  // namespace rtt::ai::stp::computations
 #endif  // RTT_PASSCOMPUTATIONS_H

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

@@ -14,6 +14,7 @@ class BallPlacer : public Role {
      * @param name The name of the role
      */
     BallPlacer(std::string name);
+    [[nodiscard]] Status update(StpInfo const& info) noexcept override;
 };
 }  // namespace rtt::ai::stp::role
 

roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h

@@ -72,15 +72,15 @@ class KeeperBlockBall : public Tactic {
      * @param info the StpInfo struct
      * @return Target position for the keeper and a bool indicating if the keeper should avoid goal posts
      */
-    static std::pair<Vector2, bool> calculateTargetPosition(const StpInfo info) noexcept;
+    static std::tuple<Vector2, bool, double> calculateTargetPosition(const StpInfo info) noexcept;
 
     /**
      * @brief Calculates the target position for the keeper when the ball is shot
      * @param info the StpInfo struct
      * @param keepersLineSegment the lineSegment of the goal
      * @param ballTrajectory the trajectory of the ball
      */
-    static Vector2 calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept;
+    static std::pair<Vector2, double> calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept;
 
     /**
      * @brief Calculates the target position for the keeper when the ball is not shot

roboteam_ai/include/roboteam_ai/utilities/Constants.h

@@ -46,14 +46,18 @@ constexpr double MIN_YAW = -M_PI;             /**< Minimum yaw the robot can hav
 constexpr double MAX_YAW = M_PI;              /**< Maximum yaw the robot can have */
 constexpr double MAX_ACC = 3.5;               /**< Maximum acceleration of the robot */
 constexpr double MAX_VEL = 4.0;               /**< Maximum allowed velocity of the robot */
+constexpr double MAX_JERK_OVERSHOOT = 100;    /**< Jerk limit for overshoot */
+// TODO ROBOCUP 2024: FIX THIS MAGIC
+constexpr double MAX_JERK_DEFAULT = 6; /**< Default jerk limit */
 
 /// GoToPos Constants
 // Distance margin for 'goToPos'. If the robot is within this margin, goToPos is successful
-constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot to be considered to have reached a position */
+constexpr double GO_TO_POS_ERROR_MARGIN = 0.02; /**< Distance error for a robot to be considered to have reached a position */
 // Angle margin for 'goToPos'. If the robot is within this margin, goToPos is successful
 constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.01; /**< Angle error for a robot to be considered to have reached a position */
 // Maximum inaccuracy during ballplacement
-constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS - 0.02; /**< Distance error for the ball to be considered to have reached the ball placement position*/
+constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS - 0.01; /**< Distance error for the ball to be considered to have reached the ball placement position*/
+constexpr double BALL_PLACER_MARGIN = BALL_PLACEMENT_MARGIN - 0.05; /**< Distance before the ball placer moves away from hte ball*/
 constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9;           /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */
 
 /// Invariant constants
@@ -64,21 +68,24 @@ constexpr double FUZZY_DEFAULT_CUTOFF = 127; /**< Value at which the fuzzy logic
 /// Distance constants
 constexpr double ROBOT_CLOSE_TO_POINT = 0.2;        /**< Distance from the robot to a position at which the robot is considered close to that position */
 constexpr double DISTANCE_TO_PASS_TRAJECTORY = 0.5; /**< Distance from the robot to the pass trajectory at which the robot is considered too close to the pass trajectory */
-constexpr double OUT_OF_FIELD_MARGIN = 0.17;      /**< Distance that the center of the robot is allowed to go out of the field during play (not for end location, only for paths) */
+constexpr double OUT_OF_FIELD_MARGIN = 0.2;       /**< Distance that the center of the robot is allowed to go out of the field during play (not for end location, only for paths) */
 constexpr double FREE_KICK_TAKEN_DISTANCE = 0.07; /**< Distance that the ball must travel before we can say a kickoff or free kick has been taken */
 constexpr double PENALTY_DISTANCE_BEHIND_BALL = 1.5; /**< The minimum distance the robots have to be behind the ball during a penalty, default is 1 meter, but do 1.5 to be sure */
 ;
 
 /// GameState constants
-constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.1; /**< Minimum distance all robots should keep when avoiding the ball */
+constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + BALL_RADIUS + 0.05; /**< Minimum distance all robots should keep when avoiding the ball */
 constexpr double AVOID_BALL_DISTANCE_BEFORE_FREE_KICK =
-    0.05 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.01; /**< Minimum distance all robots should keep when avoiding the ball before a free kick */
+    0.05 + ROBOT_RADIUS + BALL_RADIUS + 0.01; /**< Minimum distance all robots should keep when avoiding the ball before a free kick */
 
 /// Friction constants
 constexpr static float SIMULATION_FRICTION = 0.71; /**< The expected movement friction of the ball during simulation */
 constexpr static float REAL_FRICTION = 0.44;       /**< The expected movement friction of the ball on the field */
 
 static inline double HAS_BALL_DISTANCE() { return (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? 0.11 : 0.12; }
+static inline double SEND_TIME_IN_FUTURE() {
+    return (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? 0.04 : 0.145;
+}  // Simulator doesn't have mass feedforward, so we need to send a bigger difference
 
 static std::map<int, bool> ROBOTS_WITH_WORKING_DRIBBLER() {
     static std::map<int, bool> workingDribblerRobots;

roboteam_ai/include/roboteam_ai/utilities/Dealer.h

@@ -145,9 +145,10 @@ class Dealer {
      * @param robot the robot for which the cost will be determined
      * @param target_position the position the robot will be travelling to
      * @param MaxRobotVelocity the maximum velocity a robot can have
+     * @param MaxRobotJerk the maximum jerk a robot can have
      * @return Cost of travelling that distance
      */
-    static double costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity);
+    static double costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity, const double MaxRobotJerk);
 
     /**
      * @brief Calculates the cost of a property of a robot

roboteam_ai/src/control/ControlModule.cpp

@@ -12,6 +12,8 @@ namespace rtt::ai::control {
 void ControlModule::rotateRobotCommand(rtt::RobotCommand& command) {
     command.velocity.x = -command.velocity.x;
     command.velocity.y = -command.velocity.y;
+    command.acceleration.x = -command.acceleration.x;
+    command.acceleration.y = -command.acceleration.y;
     command.yaw += M_PI;
 }
 

roboteam_ai/src/control/ControlUtils.cpp

@@ -23,7 +23,7 @@ double ControlUtils::determineKickForce(const double distance, stp::ShotPower sh
 
     double kickForce;
     if (shotPower == stp::ShotPower::PASS) {
-        kickForce = 1.8;
+        kickForce = 0.8;
     } else if (shotPower == stp::ShotPower::TARGET) {
         kickForce = 0.5;
     } else {