Tune Pass Speed with Pass Distance

src/extlibs/er_force_sim/src/amun/simulator/BUILD

@@ -10,6 +10,7 @@ qt_cc_library(
         "//extlibs/er_force_sim/src/core:core_qt",
         "//extlibs/er_force_sim/src/protobuf:protobuf_qt",
         "//proto:ssl_simulation_cc_proto",
+        "//shared:constants",
         "@bullet",
         "@qt//:qt_core",
         "@qt//:qt_gui",

src/extlibs/er_force_sim/src/amun/simulator/simball.cpp

@@ -36,29 +36,32 @@ SimBall::SimBall(RNG *rng, btDiscreteDynamicsWorld *world)
       m_world(world),
       current_ball_state(STATIONARY),
       set_transition_speed(true),
-      rolling_speed(-1.0)
+      rolling_speed(-1.0f)
 {
     // see http://robocup.mi.fu-berlin.de/buch/rolling.pdf for correct modelling
-    m_sphere = new btSphereShape(BALL_RADIUS * SIMULATOR_SCALE);
+    m_sphere =
+        new btSphereShape(static_cast<float>(BALL_MAX_RADIUS_METERS) * SIMULATOR_SCALE);
 
     btVector3 localInertia(0, 0, 0);
     // FIXME measure inertia coefficient
-    m_sphere->calculateLocalInertia(BALL_MASS, localInertia);
+    m_sphere->calculateLocalInertia(BALL_MASS_KG, localInertia);
 
     btTransform startWorldTransform;
     startWorldTransform.setIdentity();
-    startWorldTransform.setOrigin(btVector3(0, 0, BALL_RADIUS) * SIMULATOR_SCALE);
+    startWorldTransform.setOrigin(
+        btVector3(0.0f, 0.0f, static_cast<float>(BALL_MAX_RADIUS_METERS)) *
+        SIMULATOR_SCALE);
     m_motionState = new btDefaultMotionState(startWorldTransform);
 
-    btRigidBody::btRigidBodyConstructionInfo rbInfo(BALL_MASS, m_motionState, m_sphere,
+    btRigidBody::btRigidBodyConstructionInfo rbInfo(BALL_MASS_KG, m_motionState, m_sphere,
                                                     localInertia);
 
     // parameters seem to be ignored...
     m_body = new btRigidBody(rbInfo);
     // see simulator.cpp
     // TODO (#2512): Check these values with real life
     m_body->setRestitution(BALL_RESTITUTION);
-    m_body->setFriction(BALL_SLIDING_FRICTION);
+    m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS);
 
     // \mu_r = -a / g = 0.0357 (while rolling)
     // rollingFriction in bullet is too unstable to be useful
@@ -79,10 +82,11 @@ void SimBall::begin(bool robot_collision)
     // custom implementation of rolling friction
     const btVector3 p        = m_body->getWorldTransform().getOrigin();
     const btVector3 velocity = m_body->getLinearVelocity();
-    if (p.z() < BALL_RADIUS * 1.1 * SIMULATOR_SCALE)
+    if (p.z() < static_cast<float>(BALL_MAX_RADIUS_METERS) * 1.1f * SIMULATOR_SCALE)
     {  // ball is on the ground
-        bool is_stationary = velocity.length() < STATIONARY_BALL_SPEED * SIMULATOR_SCALE;
-        bool should_roll   = velocity.length() < rolling_speed * SIMULATOR_SCALE;
+        bool is_stationary =
+            velocity.length() < STATIONARY_BALL_SPEED_METERS_PER_SECOND * SIMULATOR_SCALE;
+        bool should_roll = velocity.length() < rolling_speed * SIMULATOR_SCALE;
 
         if (robot_collision)
         {
@@ -106,15 +110,15 @@ void SimBall::begin(bool robot_collision)
         {
             case STATIONARY:
                 m_body->setLinearVelocity(btVector3(0, 0, 0));
-                m_body->setFriction(BALL_SLIDING_FRICTION);
+                m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS);
                 set_transition_speed = true;
                 break;
             case ROBOT_COLLISION:
-                m_body->setFriction(BALL_SLIDING_FRICTION);
+                m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS);
                 set_transition_speed = true;
                 break;
             case SLIDING:
-                m_body->setFriction(BALL_SLIDING_FRICTION);
+                m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS);
                 if (set_transition_speed)
                 {
                     rolling_speed =
@@ -126,13 +130,15 @@ void SimBall::begin(bool robot_collision)
 
                 // just apply rolling friction, normal friction is somehow handled by
                 // bullet
-                const btScalar rollingDeceleration = BALL_ROLLING_FRICTION_DECELERATION;
+                const btScalar rollingDeceleration =
+                    BALL_ROLLING_FRICTION_DECELERATION_METERS_PER_SECOND_SQUARED;
                 btVector3 force(velocity.x(), velocity.y(), 0.0f);
                 force.safeNormalize();
                 m_body->applyCentralImpulse(-force * rollingDeceleration *
-                                            SIMULATOR_SCALE * BALL_MASS * SUB_TIMESTEP);
+                                            SIMULATOR_SCALE * BALL_MASS_KG *
+                                            SUB_TIMESTEP);
 
-                m_body->setFriction(0.0);
+                m_body->setFriction(0.0f);
                 set_transition_speed = false;
                 break;
         }
@@ -187,11 +193,13 @@ void SimBall::begin(bool robot_collision)
             ErForceVector pos;
             coordinates::fromVision(m_move, pos);
             // move ball by hand
-            btVector3 force(pos.x, pos.y, m_move.z() + BALL_RADIUS);
+            btVector3 force(pos.x, pos.y,
+                            m_move.z() + static_cast<float>(BALL_MAX_RADIUS_METERS));
             force = force - m_body->getWorldTransform().getOrigin() / SIMULATOR_SCALE;
             m_body->activate();
-            m_body->applyCentralImpulse(force * BALL_MASS * 0.1 * SIMULATOR_SCALE);
-            m_body->setDamping(0.99, 0.99);
+            m_body->applyCentralImpulse(force * static_cast<float>(BALL_MASS_KG) * 0.1f *
+                                        SIMULATOR_SCALE);
+            m_body->setDamping(0.99f, 0.99f);
         }
         else
         {
@@ -200,7 +208,7 @@ void SimBall::begin(bool robot_collision)
                 // set position
                 ErForceVector cPos;
                 coordinates::fromVision(m_move, cPos);
-                float height = BALL_RADIUS;
+                float height = static_cast<float>(BALL_MAX_RADIUS_METERS);
                 if (m_move.has_z())
                 {
                     height += m_move.z();
@@ -231,7 +239,7 @@ void SimBall::begin(bool robot_collision)
                 m_body->setAngularVelocity(btVector3(0, 0, 0));
             }
             m_body->activate();
-            m_body->setDamping(0.0, 0.0);
+            m_body->setDamping(0.0f, 0.0f);
             m_move.Clear();  // clear move command
                              // reset is neccessary, as the command is only sent once
                              // without one canceling it
@@ -240,7 +248,7 @@ void SimBall::begin(bool robot_collision)
     else
     {
         // ball is slowed down by rolling friction, not damping!
-        m_body->setDamping(0.0, 0.0);
+        m_body->setDamping(0.0f, 0.0f);
     }
 }
 
@@ -250,7 +258,8 @@ static float positionOfVisiblePixels(btVector3 &p, const btVector3 &simulatorBal
                                      const btVector3 &simulatorCameraPosition,
                                      const btCollisionWorld *const m_world)
 {
-    const float simulatorBallRadius = BALL_RADIUS * SIMULATOR_SCALE;
+    const float simulatorBallRadius =
+        static_cast<float>(BALL_MAX_RADIUS_METERS) * SIMULATOR_SCALE;
 
     // axis and angle for rotating the plane towards the camera
     btVector3 cameraDirection =
@@ -337,9 +346,9 @@ bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, flo
                            btVector3 positionOffset)
 {
     // setup ssl-vision ball detection
-    ball->set_confidence(1.0);
-    ball->set_pixel_x(0);
-    ball->set_pixel_y(0);
+    ball->set_confidence(1.0f);
+    ball->set_pixel_x(0.0f);
+    ball->set_pixel_y(0.0f);
 
     btTransform transform;
     m_motionState->getWorldTransform(transform);
@@ -348,7 +357,7 @@ bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, flo
         btVector3(cameraPosition.x(), cameraPosition.y(), cameraPosition.z()) *
         SIMULATOR_SCALE;
 
-    float visibility = 1;
+    float visibility = 1.f;
     // the camera uses the mid point of the visible pixels as the mid point of the
     // ball if some parts of the ball aren't visible the position this function
     // adjusts the position accordingly (hopefully)
@@ -365,11 +374,12 @@ bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, flo
 
     const float SCALING_LIMIT = 0.9f;
     // reflects the cameras resolution
-    const unsigned PIXEL_PER_AREA = 10;  // i do not know in what unit area is,
+    const float PIXEL_PER_AREA = 10.0f;  // i do not know in what unit area is,
                                          // just make it similar to a real game
+    const float BALL_MAX_RADIUS_METERS_FLOAT = static_cast<float>(BALL_MAX_RADIUS_METERS);
 
     float modZ = std::min(SCALING_LIMIT * cameraPosition.z(),
-                          std::max(0.f, pos.z() - BALL_RADIUS));
+                          std::max(0.f, pos.z() - BALL_MAX_RADIUS_METERS_FLOAT));
     float modX = (pos.x() - cameraPosition.x()) *
                      (cameraPosition.z() / (cameraPosition.z() - modZ)) +
                  cameraPosition.x();
@@ -381,9 +391,10 @@ bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, flo
         std::sqrt((cameraPosition.z() - modZ) * (cameraPosition.z() - modZ) +
                   (cameraPosition.x() - pos.x()) * (cameraPosition.x() - pos.x()) +
                   (cameraPosition.y() - pos.y()) * (cameraPosition.y() - pos.y()));
-    float denomSqrt = (distBallCam * 1000) / FOCAL_LENGTH - 1;
+    float denomSqrt = (distBallCam * 1000.f) / FOCAL_LENGTH - 1.f;
     float basePixelArea =
-        (BALL_RADIUS * BALL_RADIUS * 1000000 * M_PI) / (denomSqrt * denomSqrt);
+        (BALL_MAX_RADIUS_METERS_FLOAT * BALL_MAX_RADIUS_METERS_FLOAT * 1000000 * M_PI) /
+        (denomSqrt * denomSqrt);
     float area =
         visibility *
         std::max(0.0f, (basePixelArea +

src/extlibs/er_force_sim/src/amun/simulator/simball.h

@@ -27,21 +27,9 @@
 
 #include "extlibs/er_force_sim/src/protobuf/command.pb.h"
 #include "extlibs/er_force_sim/src/protobuf/sslsim.h"
+#include "shared/constants.h"
 #include "simfield.h"
 
-static const float BALL_RADIUS = 0.0215f;
-static const float BALL_MASS   = 0.046f;
-
-// these values are set in coordination with other objects the ball will collide with.
-// the resulting coefficient of friction is the product of both objects friction value.
-static constexpr float BALL_SLIDING_FRICTION = 1.f;
-static constexpr float BALL_RESTITUTION      = 1.f;
-
-static constexpr float BALL_ROLLING_FRICTION_DECELERATION = 0.5;
-static constexpr float FRICTION_TRANSITION_FACTOR         = 5.0 / 7.0;
-static constexpr float STATIONARY_BALL_SPEED              = 0.01;
-
-
 class RNG;
 namespace SSLProto
 {

src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp

@@ -412,16 +412,24 @@ void SimRobot::begin(SimBall *ball, double time)
     if (m_isCharged && m_sslCommand.has_kick_speed() && m_sslCommand.kick_speed() > 0 &&
         canKickBall(ball))
     {
-        float power          = 0.0;
-        const float angle    = m_sslCommand.kick_angle() / 180 * M_PI;
-        const float dirFloor = std::cos(angle);
-        const float dirUp    = std::sin(angle);
+        float power                         = 0.0;
+        const float angle                   = m_sslCommand.kick_angle() / 180 * M_PI;
+        const float dirFloor                = std::cos(angle);
+        const float dirUp                   = std::sin(angle);
+        const float kickSpeedBoundThreshold = 1.0;
 
         stopDribbling();
 
         if (m_sslCommand.kick_angle() == 0)
         {
-            power = qBound(0.05f, m_sslCommand.kick_speed(), m_specs.shot_linear_max());
+            // we subtract the current speed of the ball from the intended kick speed
+            // this ensures the ball leaves the robot at exactly the speed we want
+            power = qBound(0.05f,
+                           ball->speed().length() < kickSpeedBoundThreshold
+                               ? m_sslCommand.kick_speed() -
+                                     (ball->speed().length() / SIMULATOR_SCALE)
+                               : m_sslCommand.kick_speed(),
+                           m_specs.shot_linear_max());
         }
         else
         {
@@ -448,7 +456,8 @@ void SimRobot::begin(SimBall *ball, double time)
         const float speedCompensation = getSpeedCompensation();
 
         ball->kick(t * btVector3(0, dirFloor * power + speedCompensation, dirUp * power) *
-                   (1 / time) * SIMULATOR_SCALE * BALL_MASS);
+                   (1.0f / static_cast<float>(time)) * SIMULATOR_SCALE *
+                   static_cast<float>(BALL_MASS_KG));
         // discharge
         m_isCharged = false;
         m_shootTime = 0.0;

src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp

@@ -977,8 +977,9 @@ void Simulator::teleportRobotToFreePosition(SimRobot *robot)
     btVector3 robotPos = robot->position() / SIMULATOR_SCALE;
     btVector3 direction =
         (robotPos - m_data->ball->position() / SIMULATOR_SCALE).normalize();
-    float distance = 2 * (BALL_RADIUS + robot->specs().radius());
-    bool valid     = true;
+    float distance =
+        2.0f * (static_cast<float>(BALL_MAX_RADIUS_METERS) + robot->specs().radius());
+    bool valid = true;
     do
     {
         valid    = true;

src/extlibs/er_force_sim/src/amun/simulator/simulator.h

@@ -39,7 +39,7 @@
 const float SIMULATOR_SCALE  = 10.0f;
 const float SUB_TIMESTEP     = 1 / 200.f;
 const float COLLISION_MARGIN = 0.04f;
-const unsigned FOCAL_LENGTH  = 390;
+const float FOCAL_LENGTH     = 390.f;
 
 
 class QByteArray;

src/proto/message_translation/tbots_protobuf.py

@@ -11,6 +11,7 @@ def create_world_state(
     blue_robot_locations: list[tbots_cpp.Point],
     ball_location: tbots_cpp.Point,
     ball_velocity: tbots_cpp.Vector,
+    blue_robot_orientations: list[float] = [],
 ) -> WorldState:
     """Initializes the world from a list of robot locations and ball location/velocity.
 
@@ -20,6 +21,7 @@ def create_world_state(
     :param blue_robot_locations: A list of blue robot locations
     :param ball_location: Location of the ball
     :param ball_velocity: Velocity of the ball
+    :param blue_robot_orientations: A list of blue robots orientations
 
     """
     world_state = WorldState()
@@ -35,11 +37,17 @@ def create_world_state(
         )
 
     for robot_id, robot_location in enumerate(blue_robot_locations):
+        orientation = 0
+        try:
+            orientation = blue_robot_orientations[robot_id]
+        except IndexError:
+            pass
         world_state.blue_robots[robot_id].CopyFrom(
             RobotState(
                 global_position=Point(
                     x_meters=robot_location.x(), y_meters=robot_location.y()
                 ),
+                global_orientation=Angle(radians=orientation),
             )
         )
 

src/proto/parameters.proto

@@ -114,7 +114,7 @@ message PassingConfig
     ];
     // The minimum pass speed (in m/s)
     required double min_pass_speed_m_per_s = 6
-        [default = 4.3, (bounds).min_double_value = 0.0, (bounds).max_double_value = 5.0];
+        [default = 1, (bounds).min_double_value = 0.0, (bounds).max_double_value = 5.0];
     // The maximum pass speed (in m/s)
     required double max_pass_speed_m_per_s = 7 [
         default                   = 6.0,
@@ -130,6 +130,11 @@ message PassingConfig
         [default = 2, (bounds).min_int_value = 0, (bounds).max_int_value = 100];
     // Cost visualization parameters
     required CostVisualizationConfig cost_vis_config = 10;
+    // The max speed at which a pass should be received
+    required double max_receive_speed = 11
+        [default = 2.0, (bounds).min_double_value = 1.0, (bounds).max_double_value = 4.0];
+    required double backwards_pass_distance = 13
+        [default = 0.5, (bounds).min_double_value = 0.0, (bounds).max_double_value = 2.0];
 }
 
 message GoalieTacticConfig

src/shared/constants.h

@@ -67,7 +67,7 @@ static const double BALL_MAX_RADIUS_METERS = 0.0215;
 static const double MAX_FRACTION_OF_BALL_COVERED_BY_ROBOT = 0.2;
 
 // The mass of a standard golf ball, as defined by https://en.wikipedia.org/wiki/Golf_ball
-static const double BALL_MASS_KG = 0.004593;
+constexpr double BALL_MASS_KG = 0.004593;
 // The max allowed speed of the robot when the stop command is issued, in meters per
 // second
 static const double STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND = 1.5;
@@ -83,6 +83,25 @@ static const double ENEMY_ROBOT_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 4.0
 
 static const double ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED = 9.81;
 
+/*Simulator Constants*/
+
+// these values are set in coordination with other objects the ball will collide with.
+// the resulting coefficient of friction is the product of both objects friction value.
+
+// the sliding friction the ball experiences in Newtons
+constexpr double BALL_SLIDING_FRICTION_NEWTONS = 1.0;
+// the restitution of the ball
+constexpr double BALL_RESTITUTION = 1.0;
+// the transition factor where the ball goes from sliding to rolling
+constexpr double FRICTION_TRANSITION_FACTOR = 5.0 / 7.0;
+// the stationary ball speed in m/s
+constexpr double STATIONARY_BALL_SPEED_METERS_PER_SECOND = 0.01;
+// the deceleration the ball experiences due to rolling friction in m/s^2
+constexpr double BALL_ROLLING_FRICTION_DECELERATION_METERS_PER_SECOND_SQUARED = 0.5;
+// the deceleration the ball experiences due to sliding friction in m/s^2
+constexpr double BALL_SLIDING_FRICTION_DECELERATION_METERS_PER_SECOND_SQUARED =
+    BALL_SLIDING_FRICTION_NEWTONS / BALL_MASS_KG;
+
 /* Unit Conversion */
 static const double MILLIMETERS_PER_METER = 1000.0;
 static const double METERS_PER_MILLIMETER = 1.0 / 1000.0;

src/software/BUILD

@@ -83,6 +83,11 @@ pybind_library(
         "//proto/message_translation:ssl_geometry",
         "//proto/message_translation:tbots_geometry",
         "//shared:robot_constants",
+        "//software:constants",
+        "//software/ai/passing:eighteen_zone_pitch_division",
+        "//software/ai/passing:pass",
+        "//software/ai/passing:pass_evaluation",
+        "//software/ai/passing:pass_generator",
         "//software/estop:threaded_estop_reader",
         "//software/geom:angle",
         "//software/geom:point",